Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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 <diff_drive_controller/odometry.h>
00050 #include <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& );
00097
00098 private:
00099 std::string name_;
00100
00102 ros::Duration publish_period_;
00103 ros::Time last_state_publish_time_;
00104 bool open_loop_;
00105
00107 std::vector<hardware_interface::JointHandle> left_wheel_joints_;
00108 std::vector<hardware_interface::JointHandle> right_wheel_joints_;
00109
00111 struct Commands
00112 {
00113 double lin;
00114 double ang;
00115 ros::Time stamp;
00116
00117 Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
00118 };
00119 realtime_tools::RealtimeBuffer<Commands> command_;
00120 Commands command_struct_;
00121 ros::Subscriber sub_command_;
00122
00124 boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
00125 boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
00126 Odometry odometry_;
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
00151 Commands last1_cmd_;
00152 Commands last0_cmd_;
00153 SpeedLimiter limiter_lin_;
00154 SpeedLimiter limiter_ang_;
00155
00156 private:
00160 void brake();
00161
00166 void cmdVelCallback(const geometry_msgs::Twist& command);
00167
00176 bool getWheelNames(ros::NodeHandle& controller_nh,
00177 const std::string& wheel_param,
00178 std::vector<std::string>& wheel_names);
00179
00186 bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
00187 const std::string& left_wheel_name,
00188 const std::string& right_wheel_name,
00189 bool lookup_wheel_separation,
00190 bool lookup_wheel_radius);
00191
00197 void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
00198
00199 };
00200
00201 PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase);
00202 }