gazebo_ros_diff_drive.cpp
Go to the documentation of this file.
1 /*
2  Copyright (c) 2010, Daniel Hewlett, Antons Rebguns
3  All rights reserved.
4 
5  Redistribution and use in source and binary forms, with or without
6  modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the <organization> nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16  THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
17  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
20  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 */
28 
29 /*
30  * \file gazebo_ros_diff_drive.cpp
31  *
32  * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin
33  * developed for the erratic robot (see copyright notice above). The original
34  * plugin can be found in the ROS package gazebo_erratic_plugins.
35  *
36  * \author Piyush Khandelwal (piyushk@gmail.com)
37  *
38  * $ Id: 06/21/2013 11:23:40 AM piyushk $
39  */
40 
41 
42 /*
43  *
44  * The support of acceleration limit was added by
45  * \author George Todoran <todorangrg@gmail.com>
46  * \author Markus Bader <markus.bader@tuwien.ac.at>
47  * \date 22th of May 2014
48  */
49 
50 #include <algorithm>
51 #include <assert.h>
52 
54 
55 #include <ignition/math/Angle.hh>
56 #include <ignition/math/Pose3.hh>
57 #include <ignition/math/Quaternion.hh>
58 #include <ignition/math/Vector3.hh>
59 #include <sdf/sdf.hh>
60 
61 #include <ros/ros.h>
62 
63 namespace gazebo
64 {
65 
66 enum {
69 };
70 
72 
73 // Destructor
75 {
76  FiniChild();
77 }
78 
79 // Load the controller
80 void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
81 {
82 
83  this->parent = _parent;
84  gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "DiffDrive" ) );
85  // Make sure the ROS node for Gazebo has already been initialized
86  gazebo_ros_->isInitialized();
87 
88  gazebo_ros_->getParameter<std::string> ( command_topic_, "commandTopic", "cmd_vel" );
89  gazebo_ros_->getParameter<std::string> ( odometry_topic_, "odometryTopic", "odom" );
90  gazebo_ros_->getParameter<std::string> ( odometry_frame_, "odometryFrame", "odom" );
91  gazebo_ros_->getParameter<std::string> ( robot_base_frame_, "robotBaseFrame", "base_footprint" );
92  gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false );
93  gazebo_ros_->getParameterBoolean ( publishOdomTF_, "publishOdomTF", true);
94  gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false );
95  gazebo_ros_->getParameterBoolean ( legacy_mode_, "legacyMode", true );
96 
97  if (!_sdf->HasElement("legacyMode"))
98  {
99  ROS_ERROR_NAMED("diff_drive", "GazeboRosDiffDrive Plugin missing <legacyMode>, defaults to true\n"
100  "This setting assumes you have a old package, where the right and left wheel are changed to fix a former code issue\n"
101  "To get rid of this error just set <legacyMode> to false if you just created a new package.\n"
102  "To fix an old package you have to exchange left wheel by the right wheel.\n"
103  "If you do not want to fix this issue in an old package or your z axis points down instead of the ROS standard defined in REP 103\n"
104  "just set <legacyMode> to true.\n"
105  );
106  }
107 
108  gazebo_ros_->getParameter<double> ( wheel_separation_, "wheelSeparation", 0.34 );
109  gazebo_ros_->getParameter<double> ( wheel_diameter_, "wheelDiameter", 0.15 );
110  gazebo_ros_->getParameter<double> ( wheel_accel, "wheelAcceleration", 0.0 );
111  gazebo_ros_->getParameter<double> ( wheel_torque, "wheelTorque", 5.0 );
112  gazebo_ros_->getParameter<double> ( update_rate_, "updateRate", 100.0 );
113  std::map<std::string, OdomSource> odomOptions;
114  odomOptions["encoder"] = ENCODER;
115  odomOptions["world"] = WORLD;
116  gazebo_ros_->getParameter<OdomSource> ( odom_source_, "odometrySource", odomOptions, WORLD );
117 
118 
119  joints_.resize ( 2 );
120  joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" );
121  joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" );
122  joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque );
123  joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque );
124 
125 
126 
127  this->publish_tf_ = true;
128  if (!_sdf->HasElement("publishTf")) {
129  ROS_WARN_NAMED("diff_drive", "GazeboRosDiffDrive Plugin (ns = %s) missing <publishTf>, defaults to %d",
130  this->robot_namespace_.c_str(), this->publish_tf_);
131  } else {
132  this->publish_tf_ = _sdf->GetElement("publishTf")->Get<bool>();
133  }
134 
135  // Initialize update rate stuff
136  if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_;
137  else this->update_period_ = 0.0;
138 #if GAZEBO_MAJOR_VERSION >= 8
139  last_update_time_ = parent->GetWorld()->SimTime();
140 #else
141  last_update_time_ = parent->GetWorld()->GetSimTime();
142 #endif
143 
144  // Initialize velocity stuff
145  wheel_speed_[RIGHT] = 0;
146  wheel_speed_[LEFT] = 0;
147 
148  // Initialize velocity support stuff
151 
152  x_ = 0;
153  rot_ = 0;
154  alive_ = true;
155 
156 
157  if (this->publishWheelJointState_)
158  {
159  joint_state_publisher_ = gazebo_ros_->node()->advertise<sensor_msgs::JointState>("joint_states", 1000);
160  ROS_INFO_NAMED("diff_drive", "%s: Advertise joint_states", gazebo_ros_->info());
161  }
162 
164 
165  // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
166  ROS_INFO_NAMED("diff_drive", "%s: Try to subscribe to %s", gazebo_ros_->info(), command_topic_.c_str());
167 
169  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
170  boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1),
171  ros::VoidPtr(), &queue_);
172 
173  cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so);
174  ROS_INFO_NAMED("diff_drive", "%s: Subscribe to %s", gazebo_ros_->info(), command_topic_.c_str());
175 
176  if (this->publish_tf_)
177  {
178  odometry_publisher_ = gazebo_ros_->node()->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
179  ROS_INFO_NAMED("diff_drive", "%s: Advertise odom on %s ", gazebo_ros_->info(), odometry_topic_.c_str());
180  }
181 
182  // start custom queue for diff drive
183  this->callback_queue_thread_ =
184  boost::thread ( boost::bind ( &GazeboRosDiffDrive::QueueThread, this ) );
185 
186  // listen to the update event (broadcast every simulation iteration)
187  this->update_connection_ =
188  event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosDiffDrive::UpdateChild, this ) );
189 
190 }
191 
193 {
194 #if GAZEBO_MAJOR_VERSION >= 8
195  last_update_time_ = parent->GetWorld()->SimTime();
196 #else
197  last_update_time_ = parent->GetWorld()->GetSimTime();
198 #endif
199  pose_encoder_.x = 0;
200  pose_encoder_.y = 0;
201  pose_encoder_.theta = 0;
202  x_ = 0;
203  rot_ = 0;
204  joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque );
205  joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque );
206 }
207 
209 {
210  ros::Time current_time = ros::Time::now();
211 
212  joint_state_.header.stamp = current_time;
213  joint_state_.name.resize ( joints_.size() );
214  joint_state_.position.resize ( joints_.size() );
215 
216  for ( int i = 0; i < 2; i++ ) {
217  physics::JointPtr joint = joints_[i];
218 #if GAZEBO_MAJOR_VERSION >= 8
219  double position = joint->Position ( 0 );
220 #else
221  double position = joint->GetAngle ( 0 ).Radian();
222 #endif
223  joint_state_.name[i] = joint->GetName();
224  joint_state_.position[i] = position;
225  }
227 }
228 
230 {
231  ros::Time current_time = ros::Time::now();
232  for ( int i = 0; i < 2; i++ ) {
233 
234  std::string wheel_frame = gazebo_ros_->resolveTF(joints_[i]->GetChild()->GetName ());
235  std::string wheel_parent_frame = gazebo_ros_->resolveTF(joints_[i]->GetParent()->GetName ());
236 
237 #if GAZEBO_MAJOR_VERSION >= 8
238  ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->RelativePose();
239 #else
240  ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->GetRelativePose().Ign();
241 #endif
242 
243  tf::Quaternion qt ( poseWheel.Rot().X(), poseWheel.Rot().Y(), poseWheel.Rot().Z(), poseWheel.Rot().W() );
244  tf::Vector3 vt ( poseWheel.Pos().X(), poseWheel.Pos().Y(), poseWheel.Pos().Z() );
245 
246  tf::Transform tfWheel ( qt, vt );
247  transform_broadcaster_->sendTransform (
248  tf::StampedTransform ( tfWheel, current_time, wheel_parent_frame, wheel_frame ) );
249  }
250 }
251 
252 // Update the controller
254 {
255 
256  /* force reset SetParam("fmax") since Joint::Reset reset MaxForce to zero at
257  https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331
258  (this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 )
259  and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset
260  (this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e)
261  */
262  for ( int i = 0; i < 2; i++ ) {
263  if ( fabs(wheel_torque -joints_[i]->GetParam ( "fmax", 0 )) > 1e-6 ) {
264  joints_[i]->SetParam ( "fmax", 0, wheel_torque );
265  }
266  }
267 
268 
270 #if GAZEBO_MAJOR_VERSION >= 8
271  common::Time current_time = parent->GetWorld()->SimTime();
272 #else
273  common::Time current_time = parent->GetWorld()->GetSimTime();
274 #endif
275  double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
276 
277  if ( seconds_since_last_update > update_period_ ) {
278  if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
281 
282  // Update robot in case new velocities have been requested
284 
285  double current_speed[2];
286 
287  current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
288  current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
289 
290  if ( wheel_accel == 0 ||
291  ( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) ||
292  ( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) {
293  //if max_accel == 0, or target speed is reached
294  joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
295  joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
296  } else {
297  if ( wheel_speed_[LEFT]>=current_speed[LEFT] )
298  wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update );
299  else
300  wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update );
301 
302  if ( wheel_speed_[RIGHT]>current_speed[RIGHT] )
303  wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update );
304  else
305  wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update );
306 
307  // ROS_INFO_NAMED("diff_drive", "actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
308  // ROS_INFO_NAMED("diff_drive", "actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
309 
310  joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
311  joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
312  }
313  last_update_time_+= common::Time ( update_period_ );
314  }
315 }
316 
317 // Finalize the controller
319 {
320  alive_ = false;
321  queue_.clear();
322  queue_.disable();
323  gazebo_ros_->node()->shutdown();
324  callback_queue_thread_.join();
325 }
326 
328 {
329  boost::mutex::scoped_lock scoped_lock ( lock );
330 
331  double vr = x_;
332  double va = rot_;
333 
334  if(legacy_mode_)
335  {
336  wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0;
337  wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0;
338  }
339  else
340  {
341  wheel_speed_[LEFT] = vr - va * wheel_separation_ / 2.0;
342  wheel_speed_[RIGHT] = vr + va * wheel_separation_ / 2.0;
343  }
344 }
345 
346 void GazeboRosDiffDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg )
347 {
348  boost::mutex::scoped_lock scoped_lock ( lock );
349  x_ = cmd_msg->linear.x;
350  rot_ = cmd_msg->angular.z;
351 }
352 
354 {
355  static const double timeout = 0.01;
356 
357  while ( alive_ && gazebo_ros_->node()->ok() ) {
358  queue_.callAvailable ( ros::WallDuration ( timeout ) );
359  }
360 }
361 
363 {
364  double vl = joints_[LEFT]->GetVelocity ( 0 );
365  double vr = joints_[RIGHT]->GetVelocity ( 0 );
366 #if GAZEBO_MAJOR_VERSION >= 8
367  common::Time current_time = parent->GetWorld()->SimTime();
368 #else
369  common::Time current_time = parent->GetWorld()->GetSimTime();
370 #endif
371  double seconds_since_last_update = ( current_time - last_odom_update_ ).Double();
372  last_odom_update_ = current_time;
373 
374  double b = wheel_separation_;
375 
376  // Book: Sigwart 2011 Autonompus Mobile Robots page:337
377  double sl = vl * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
378  double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
379  double ssum = sl + sr;
380 
381  double sdiff;
382  if(legacy_mode_)
383  {
384  sdiff = sl - sr;
385  }
386  else
387  {
388 
389  sdiff = sr - sl;
390  }
391 
392  double dx = ( ssum ) /2.0 * cos ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
393  double dy = ( ssum ) /2.0 * sin ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) );
394  double dtheta = ( sdiff ) /b;
395 
396  pose_encoder_.x += dx;
397  pose_encoder_.y += dy;
398  pose_encoder_.theta += dtheta;
399 
400  double w = dtheta/seconds_since_last_update;
401  double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;
402 
403  tf::Quaternion qt;
404  tf::Vector3 vt;
405  qt.setRPY ( 0,0,pose_encoder_.theta );
406  vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 );
407 
408  odom_.pose.pose.position.x = vt.x();
409  odom_.pose.pose.position.y = vt.y();
410  odom_.pose.pose.position.z = vt.z();
411 
412  odom_.pose.pose.orientation.x = qt.x();
413  odom_.pose.pose.orientation.y = qt.y();
414  odom_.pose.pose.orientation.z = qt.z();
415  odom_.pose.pose.orientation.w = qt.w();
416 
417  odom_.twist.twist.angular.z = w;
418  odom_.twist.twist.linear.x = dx/seconds_since_last_update;
419  odom_.twist.twist.linear.y = dy/seconds_since_last_update;
420 }
421 
422 void GazeboRosDiffDrive::publishOdometry ( double step_time )
423 {
424 
425  ros::Time current_time = ros::Time::now();
426  std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ );
427  std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ );
428 
429  tf::Quaternion qt;
430  tf::Vector3 vt;
431 
432  if ( odom_source_ == ENCODER ) {
433  // getting data form encoder integration
434  qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w );
435  vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z );
436 
437  }
438  if ( odom_source_ == WORLD ) {
439  // getting data from gazebo world
440 #if GAZEBO_MAJOR_VERSION >= 8
441  ignition::math::Pose3d pose = parent->WorldPose();
442 #else
443  ignition::math::Pose3d pose = parent->GetWorldPose().Ign();
444 #endif
445  qt = tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
446  vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
447 
448  odom_.pose.pose.position.x = vt.x();
449  odom_.pose.pose.position.y = vt.y();
450  odom_.pose.pose.position.z = vt.z();
451 
452  odom_.pose.pose.orientation.x = qt.x();
453  odom_.pose.pose.orientation.y = qt.y();
454  odom_.pose.pose.orientation.z = qt.z();
455  odom_.pose.pose.orientation.w = qt.w();
456 
457  // get velocity in /odom frame
458  ignition::math::Vector3d linear;
459 #if GAZEBO_MAJOR_VERSION >= 8
460  linear = parent->WorldLinearVel();
461  odom_.twist.twist.angular.z = parent->WorldAngularVel().Z();
462 #else
463  linear = parent->GetWorldLinearVel().Ign();
464  odom_.twist.twist.angular.z = parent->GetWorldAngularVel().Ign().Z();
465 #endif
466 
467  // convert velocity to child_frame_id (aka base_footprint)
468  float yaw = pose.Rot().Yaw();
469  odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y();
470  odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X();
471  }
472 
473  if (publishOdomTF_ == true){
474  tf::Transform base_footprint_to_odom ( qt, vt );
475  transform_broadcaster_->sendTransform (
476  tf::StampedTransform ( base_footprint_to_odom, current_time,
477  odom_frame, base_footprint_frame ) );
478  }
479 
480 
481  // set covariance
482  odom_.pose.covariance[0] = 0.00001;
483  odom_.pose.covariance[7] = 0.00001;
484  odom_.pose.covariance[14] = 1000000000000.0;
485  odom_.pose.covariance[21] = 1000000000000.0;
486  odom_.pose.covariance[28] = 1000000000000.0;
487  odom_.pose.covariance[35] = 0.001;
488 
489 
490  // set header
491  odom_.header.stamp = current_time;
492  odom_.header.frame_id = odom_frame;
493  odom_.child_frame_id = base_footprint_frame;
494 
496 }
497 
499 }
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
TFSIMD_FORCE_INLINE const tfScalar & x() const
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
event::ConnectionPtr update_connection_
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
sensor_msgs::JointState joint_state_
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
void publishOdometry(double step_time)
#define ROS_ERROR_NAMED(name,...)
static Time now()
void publishWheelJointState()
publishes the wheel tf&#39;s
geometry_msgs::Pose2D pose_encoder_
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
boost::shared_ptr< void > VoidPtr
boost::shared_ptr< GazeboRos > GazeboRosPtr
std::vector< physics::JointPtr > joints_


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39