diff_drive_controller.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the PAL Robotics nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036  * Author: Enrique Fernández
00037  */
00038 
00039 #include <controller_interface/controller.h>
00040 #include <hardware_interface/joint_command_interface.h>
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 #include <nav_msgs/Odometry.h>
00044 #include <tf/tfMessage.h>
00045 
00046 #include <realtime_tools/realtime_buffer.h>
00047 #include <realtime_tools/realtime_publisher.h>
00048 
00049 #include <jackal_diff_drive_controller/odometry.h>
00050 #include <jackal_diff_drive_controller/speed_limiter.h>
00051 
00052 namespace diff_drive_controller{
00053 
00063   class DiffDriveController
00064       : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
00065   {
00066   public:
00067     DiffDriveController();
00068 
00075     bool init(hardware_interface::VelocityJointInterface* hw,
00076               ros::NodeHandle& root_nh,
00077               ros::NodeHandle &controller_nh);
00078 
00084     void update(const ros::Time& time, const ros::Duration& period);
00085 
00090     void starting(const ros::Time& time);
00091 
00096     void stopping(const ros::Time& time);
00097 
00098   private:
00099     std::string name_;
00100 
00102     ros::Duration publish_period_;
00103     ros::Time last_state_publish_time_;
00104 
00106     std::vector<hardware_interface::JointHandle> left_wheel_joints_;
00107     std::vector<hardware_interface::JointHandle> right_wheel_joints_;
00108 
00110     struct Commands
00111     {
00112       double lin;
00113       double ang;
00114       ros::Time stamp;
00115 
00116       Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
00117     };
00118     realtime_tools::RealtimeBuffer<Commands> command_;
00119     Commands command_struct_;
00120     ros::Subscriber sub_command_;
00121 
00123     boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
00124     boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
00125     Odometry odometry_;
00126     geometry_msgs::TransformStamped odom_frame_;
00127 
00129     double wheel_separation_;
00130 
00132     double wheel_radius_;
00133 
00135     double wheel_separation_multiplier_;
00136     double wheel_radius_multiplier_;
00137 
00139     double cmd_vel_timeout_;
00140 
00142     std::string base_frame_id_;
00143 
00145     bool enable_odom_tf_;
00146 
00148     size_t wheel_joints_size_;
00149 
00150     // Speed limiters:
00151     Commands last_cmd_;
00152     SpeedLimiter limiter_lin_;
00153     SpeedLimiter limiter_ang_;
00154 
00155   private:
00159     void brake();
00160 
00165     void cmdVelCallback(const geometry_msgs::Twist& command);
00166 
00175     bool getWheelNames(ros::NodeHandle& controller_nh,
00176                        const std::string& wheel_param,
00177                        std::vector<std::string>& wheel_names);
00178 
00185     bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
00186                                const std::string& left_wheel_name,
00187                                const std::string& right_wheel_name);
00188 
00194     void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
00195 
00196   };
00197 
00198   PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase);
00199 } // namespace diff_drive_controller


jackal_diff_drive_controller
Author(s): Bence Magyar
autogenerated on Fri Dec 12 2014 23:32:03