diff_drive_controller.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, PAL Robotics, S.L.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the PAL Robotics nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /*
36  * Author: Enrique Fernández
37  */
38 
42 #include <dynamic_reconfigure/server.h>
43 
44 #include <geometry_msgs/TwistStamped.h>
45 #include <nav_msgs/Odometry.h>
46 #include <tf/tfMessage.h>
47 
50 
53 #include <diff_drive_controller/DiffDriveControllerConfig.h>
54 
56 
67  : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
68  {
69  public:
71 
79  ros::NodeHandle& root_nh,
80  ros::NodeHandle &controller_nh);
81 
87  void update(const ros::Time& time, const ros::Duration& period);
88 
93  void starting(const ros::Time& time);
94 
99  void stopping(const ros::Time& /*time*/);
100 
101  private:
102  std::string name_;
103 
108 
110  std::vector<hardware_interface::JointHandle> left_wheel_joints_;
111  std::vector<hardware_interface::JointHandle> right_wheel_joints_;
112 
114  struct Commands
115  {
116  double lin;
117  double ang;
119 
120  Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
121  };
125 
128 
133 
136 
139 
144 
147 
150 
152  std::string base_frame_id_;
153 
155  std::string odom_frame_id_;
156 
159 
162 
168 
171 
172  // A struct to hold dynamic parameters
173  // set from dynamic_reconfigure server
175  {
176  bool update;
177 
181 
183 
184  double publish_rate;
186 
188  : left_wheel_radius_multiplier(1.0)
189  , right_wheel_radius_multiplier(1.0)
190  , wheel_separation_multiplier(1.0)
191  , publish_cmd(false)
192  , publish_rate(50)
193  , enable_odom_tf(true)
194  {}
195 
196  friend std::ostream& operator<<(std::ostream& os, const DynamicParams& params)
197  {
198  os << "DynamicParams:\n"
199  //
200  << "\tOdometry parameters:\n"
201  << "\t\tleft wheel radius: " << params.left_wheel_radius_multiplier << "\n"
202  << "\t\tright wheel radius: " << params.right_wheel_radius_multiplier << "\n"
203  << "\t\twheel separation: " << params.wheel_separation_multiplier << "\n"
204  //
205  << "\tPublication parameters:\n"
206  << "\t\tPublish executed velocity command: " << params.publish_cmd << "\n"
207  << "\t\tPublication rate: " << params.publish_rate << "\n"
208  << "\t\tPublish frame odom on tf: " << params.enable_odom_tf;
209 
210  return os;
211  }
212  };
213 
215 
217  typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> ReconfigureServer;
219 
220  private:
224  void brake();
225 
230  void cmdVelCallback(const geometry_msgs::Twist& command);
231 
240  bool getWheelNames(ros::NodeHandle& controller_nh,
241  const std::string& wheel_param,
242  std::vector<std::string>& wheel_names);
243 
251  const std::string& left_wheel_name,
252  const std::string& right_wheel_name,
253  bool lookup_wheel_separation,
254  bool lookup_wheel_radius);
255 
261  void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
262 
269  void reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/);
270 
274  void updateDynamicParams();
275  };
276 
278 } // namespace diff_drive_controller
realtime_tools::RealtimeBuffer< Commands > command_
bool allow_multiple_cmd_vel_publishers_
Whether to allow multiple publishers on cmd_vel topic or not:
bool init(hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
double wheel_separation_
Wheel separation, wrt the midpoint of the wheel width:
void updateDynamicParams()
Update the dynamic parameters in the RT loop.
std::vector< hardware_interface::JointHandle > left_wheel_joints_
Hardware handles:
double wheel_radius_
Wheel radius (assuming it&#39;s the same for the left and right wheels):
std::string odom_frame_id_
Frame to use for odometry and odom tf:
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
size_t wheel_joints_size_
Number of wheel joints:
bool getWheelNames(ros::NodeHandle &controller_nh, const std::string &wheel_param, std::vector< std::string > &wheel_names)
Get the wheel names from a wheel param.
PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase)
std::vector< hardware_interface::JointHandle > right_wheel_joints_
void starting(const ros::Time &time)
Starts controller.
boost::shared_ptr< ReconfigureServer > dyn_reconf_server_
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
void update(const ros::Time &time, const ros::Duration &period)
Updates controller, i.e. computes the odometry and sets the new velocity commands.
realtime_tools::RealtimeBuffer< DynamicParams > dynamic_params_
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:59
ros::Duration publish_period_
Odometry related:
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
friend std::ostream & operator<<(std::ostream &os, const DynamicParams &params)
bool setOdomParamsFromUrdf(ros::NodeHandle &root_nh, const std::string &left_wheel_name, const std::string &right_wheel_name, bool lookup_wheel_separation, bool lookup_wheel_radius)
Sets odometry parameters from the URDF, i.e. the wheel radius and separation.
bool enable_odom_tf_
Whether to publish odometry to tf or not:
void reconfCallback(DiffDriveControllerConfig &config, uint32_t)
Callback for dynamic_reconfigure server.
boost::shared_ptr< realtime_tools::RealtimePublisher< geometry_msgs::TwistStamped > > cmd_vel_pub_
Publish executed commands.
void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
dynamic_reconfigure::Server< DiffDriveControllerConfig > ReconfigureServer
Dynamic Reconfigure server.
void stopping(const ros::Time &)
Stops controller.
double wheel_separation_multiplier_
Wheel separation and radius calibration multipliers:
std::string base_frame_id_
Frame to use for the robot base:


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05