gazebo_ros_skid_steer_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_skid_steer_drive.cpp
31  *
32  * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin
33  *
34  * \author Zdenek Materna (imaterna@fit.vutbr.cz)
35  *
36  * $ Id: 06/25/2013 11:23:40 AM materna $
37  */
38 
39 
40 #include <algorithm>
41 #include <assert.h>
42 
44 
45 #include <ignition/math/Pose3.hh>
46 #include <ignition/math/Vector3.hh>
47 #include <sdf/sdf.hh>
48 
49 #include <ros/ros.h>
51 #include <tf/transform_listener.h>
52 #include <geometry_msgs/Twist.h>
53 #include <nav_msgs/GetMap.h>
54 #include <nav_msgs/Odometry.h>
55 #include <boost/bind.hpp>
56 #include <boost/thread/mutex.hpp>
57 
58 namespace gazebo {
59 
60  enum {
65  };
66 
68 
69  // Destructor
71  delete rosnode_;
73  }
74 
75  // Load the controller
76  void GazeboRosSkidSteerDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
77 
78  this->parent = _parent;
79  this->world = _parent->GetWorld();
80 
81  this->robot_namespace_ = "";
82  if (!_sdf->HasElement("robotNamespace")) {
83  ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin missing <robotNamespace>, defaults to \"%s\"",
84  this->robot_namespace_.c_str());
85  } else {
86  this->robot_namespace_ =
87  _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
88  }
89 
90  this->broadcast_tf_ = false;
91  if (!_sdf->HasElement("broadcastTF")) {
92  if (!this->broadcast_tf_)
93  ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <broadcastTF>, defaults to false.",this->robot_namespace_.c_str());
94  else ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <broadcastTF>, defaults to true.",this->robot_namespace_.c_str());
95 
96  } else {
97  this->broadcast_tf_ = _sdf->GetElement("broadcastTF")->Get<bool>();
98  }
99 
100  // TODO write error if joint doesn't exist!
101  this->left_front_joint_name_ = "left_front_joint";
102  if (!_sdf->HasElement("leftFrontJoint")) {
103  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <leftFrontJoint>, defaults to \"%s\"",
104  this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str());
105  } else {
106  this->left_front_joint_name_ = _sdf->GetElement("leftFrontJoint")->Get<std::string>();
107  }
108 
109  this->right_front_joint_name_ = "right_front_joint";
110  if (!_sdf->HasElement("rightFrontJoint")) {
111  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <rightFrontJoint>, defaults to \"%s\"",
112  this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str());
113  } else {
114  this->right_front_joint_name_ = _sdf->GetElement("rightFrontJoint")->Get<std::string>();
115  }
116 
117  this->left_rear_joint_name_ = "left_rear_joint";
118  if (!_sdf->HasElement("leftRearJoint")) {
119  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <leftRearJoint>, defaults to \"%s\"",
120  this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str());
121  } else {
122  this->left_rear_joint_name_ = _sdf->GetElement("leftRearJoint")->Get<std::string>();
123  }
124 
125  this->right_rear_joint_name_ = "right_rear_joint";
126  if (!_sdf->HasElement("rightRearJoint")) {
127  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <rightRearJoint>, defaults to \"%s\"",
128  this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str());
129  } else {
130  this->right_rear_joint_name_ = _sdf->GetElement("rightRearJoint")->Get<std::string>();
131  }
132 
133 
134  // This assumes that front and rear wheel spacing is identical
135  /*this->wheel_separation_ = this->parent->GetJoint(left_front_joint_name_)->GetAnchor(0).Distance(
136  this->parent->GetJoint(right_front_joint_name_)->GetAnchor(0));*/
137 
138  this->wheel_separation_ = 0.4;
139 
140  if (!_sdf->HasElement("wheelSeparation")) {
141  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <wheelSeparation>, defaults to value from robot_description: %f",
142  this->robot_namespace_.c_str(), this->wheel_separation_);
143  } else {
144  this->wheel_separation_ =
145  _sdf->GetElement("wheelSeparation")->Get<double>();
146  }
147 
148  // TODO get this from robot_description
149  this->wheel_diameter_ = 0.15;
150  if (!_sdf->HasElement("wheelDiameter")) {
151  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <wheelDiameter>, defaults to %f",
152  this->robot_namespace_.c_str(), this->wheel_diameter_);
153  } else {
154  this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get<double>();
155  }
156 
157  this->torque = 5.0;
158  if (!_sdf->HasElement("torque")) {
159  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <torque>, defaults to %f",
160  this->robot_namespace_.c_str(), this->torque);
161  } else {
162  this->torque = _sdf->GetElement("torque")->Get<double>();
163  }
164 
165  this->command_topic_ = "cmd_vel";
166  if (!_sdf->HasElement("commandTopic")) {
167  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <commandTopic>, defaults to \"%s\"",
168  this->robot_namespace_.c_str(), this->command_topic_.c_str());
169  } else {
170  this->command_topic_ = _sdf->GetElement("commandTopic")->Get<std::string>();
171  }
172 
173  this->odometry_topic_ = "odom";
174  if (!_sdf->HasElement("odometryTopic")) {
175  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <odometryTopic>, defaults to \"%s\"",
176  this->robot_namespace_.c_str(), this->odometry_topic_.c_str());
177  } else {
178  this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get<std::string>();
179  }
180 
181  this->odometry_frame_ = "odom";
182  if (!_sdf->HasElement("odometryFrame")) {
183  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <odometryFrame>, defaults to \"%s\"",
184  this->robot_namespace_.c_str(), this->odometry_frame_.c_str());
185  } else {
186  this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get<std::string>();
187  }
188 
189  this->robot_base_frame_ = "base_footprint";
190  if (!_sdf->HasElement("robotBaseFrame")) {
191  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <robotBaseFrame>, defaults to \"%s\"",
192  this->robot_namespace_.c_str(), this->robot_base_frame_.c_str());
193  } else {
194  this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get<std::string>();
195  }
196 
197  this->update_rate_ = 100.0;
198  if (!_sdf->HasElement("updateRate")) {
199  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <updateRate>, defaults to %f",
200  this->robot_namespace_.c_str(), this->update_rate_);
201  } else {
202  this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
203  }
204 
205  this->covariance_x_ = 0.0001;
206  if (!_sdf->HasElement("covariance_x")) {
207  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <covariance_x>, defaults to %f",
208  this->robot_namespace_.c_str(), covariance_x_);
209  } else {
210  covariance_x_ = _sdf->GetElement("covariance_x")->Get<double>();
211  }
212 
213  this->covariance_y_ = 0.0001;
214  if (!_sdf->HasElement("covariance_y")) {
215  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <covariance_y>, defaults to %f",
216  this->robot_namespace_.c_str(), covariance_y_);
217  } else {
218  covariance_y_ = _sdf->GetElement("covariance_y")->Get<double>();
219  }
220 
221  this->covariance_yaw_ = 0.01;
222  if (!_sdf->HasElement("covariance_yaw")) {
223  ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing <covariance_yaw>, defaults to %f",
224  this->robot_namespace_.c_str(), covariance_yaw_);
225  } else {
226  covariance_yaw_ = _sdf->GetElement("covariance_yaw")->Get<double>();
227  }
228 
229  // Initialize update rate stuff
230  if (this->update_rate_ > 0.0) {
231  this->update_period_ = 1.0 / this->update_rate_;
232  } else {
233  this->update_period_ = 0.0;
234  }
235 #if GAZEBO_MAJOR_VERSION >= 8
236  last_update_time_ = this->world->SimTime();
237 #else
238  last_update_time_ = this->world->GetSimTime();
239 #endif
240 
241  // Initialize velocity stuff
245  wheel_speed_[LEFT_REAR] = 0;
246 
247  x_ = 0;
248  rot_ = 0;
249  alive_ = true;
250 
251  joints[LEFT_FRONT] = this->parent->GetJoint(left_front_joint_name_);
252  joints[RIGHT_FRONT] = this->parent->GetJoint(right_front_joint_name_);
253  joints[LEFT_REAR] = this->parent->GetJoint(left_rear_joint_name_);
254  joints[RIGHT_REAR] = this->parent->GetJoint(right_rear_joint_name_);
255 
256  if (!joints[LEFT_FRONT]) {
257  char error[200];
258  snprintf(error, 200,
259  "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left front hinge joint named \"%s\"",
260  this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str());
261  gzthrow(error);
262  }
263 
264  if (!joints[RIGHT_FRONT]) {
265  char error[200];
266  snprintf(error, 200,
267  "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right front hinge joint named \"%s\"",
268  this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str());
269  gzthrow(error);
270  }
271 
272  if (!joints[LEFT_REAR]) {
273  char error[200];
274  snprintf(error, 200,
275  "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left rear hinge joint named \"%s\"",
276  this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str());
277  gzthrow(error);
278  }
279 
280  if (!joints[RIGHT_REAR]) {
281  char error[200];
282  snprintf(error, 200,
283  "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right rear hinge joint named \"%s\"",
284  this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str());
285  gzthrow(error);
286  }
287 
288 #if GAZEBO_MAJOR_VERSION > 2
289  joints[LEFT_FRONT]->SetParam("fmax", 0, torque);
290  joints[RIGHT_FRONT]->SetParam("fmax", 0, torque);
291  joints[LEFT_REAR]->SetParam("fmax", 0, torque);
292  joints[RIGHT_REAR]->SetParam("fmax", 0, torque);
293 #else
294  joints[LEFT_FRONT]->SetMaxForce(0, torque);
295  joints[RIGHT_FRONT]->SetMaxForce(0, torque);
296  joints[LEFT_REAR]->SetMaxForce(0, torque);
297  joints[RIGHT_REAR]->SetMaxForce(0, torque);
298 #endif
299 
300  // Make sure the ROS node for Gazebo has already been initialized
301  if (!ros::isInitialized())
302  {
303  ROS_FATAL_STREAM_NAMED("skid_steer_drive", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
304  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
305  return;
306  }
307 
309 
310  ROS_INFO_NAMED("skid_steer_drive", "Starting GazeboRosSkidSteerDrive Plugin (ns = %s)", this->robot_namespace_.c_str());
311 
314 
315  // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
317  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
318  boost::bind(&GazeboRosSkidSteerDrive::cmdVelCallback, this, _1),
319  ros::VoidPtr(), &queue_);
320 
322 
323  odometry_publisher_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
324 
325  // start custom queue for diff drive
326  this->callback_queue_thread_ =
327  boost::thread(boost::bind(&GazeboRosSkidSteerDrive::QueueThread, this));
328 
329  // listen to the update event (broadcast every simulation iteration)
330  this->update_connection_ =
331  event::Events::ConnectWorldUpdateBegin(
332  boost::bind(&GazeboRosSkidSteerDrive::UpdateChild, this));
333 
334  }
335 
336  // Update the controller
338 #if GAZEBO_MAJOR_VERSION >= 8
339  common::Time current_time = this->world->SimTime();
340 #else
341  common::Time current_time = this->world->GetSimTime();
342 #endif
343  double seconds_since_last_update =
344  (current_time - last_update_time_).Double();
345  if (seconds_since_last_update > update_period_) {
346 
347  publishOdometry(seconds_since_last_update);
348 
349  // Update robot in case new velocities have been requested
351 #if GAZEBO_MAJOR_VERSION > 2
352  joints[LEFT_FRONT]->SetParam("vel", 0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0));
353  joints[RIGHT_FRONT]->SetParam("vel", 0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0));
354  joints[LEFT_REAR]->SetParam("vel", 0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0));
355  joints[RIGHT_REAR]->SetParam("vel", 0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0));
356 #else
357  joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0));
358  joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0));
359  joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0));
360  joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0));
361 #endif
362 
363  last_update_time_+= common::Time(update_period_);
364 
365  }
366  }
367 
368  // Finalize the controller
370  alive_ = false;
371  queue_.clear();
372  queue_.disable();
373  rosnode_->shutdown();
374  callback_queue_thread_.join();
375  }
376 
378  boost::mutex::scoped_lock scoped_lock(lock);
379 
380  double vr = x_;
381  double va = rot_;
382 
383  wheel_speed_[RIGHT_FRONT] = vr + va * wheel_separation_ / 2.0;
384  wheel_speed_[RIGHT_REAR] = vr + va * wheel_separation_ / 2.0;
385 
386  wheel_speed_[LEFT_FRONT] = vr - va * wheel_separation_ / 2.0;
387  wheel_speed_[LEFT_REAR] = vr - va * wheel_separation_ / 2.0;
388 
389  }
390 
392  const geometry_msgs::Twist::ConstPtr& cmd_msg) {
393 
394  boost::mutex::scoped_lock scoped_lock(lock);
395  x_ = cmd_msg->linear.x;
396  rot_ = cmd_msg->angular.z;
397  }
398 
400  static const double timeout = 0.01;
401 
402  while (alive_ && rosnode_->ok()) {
404  }
405  }
406 
408  ros::Time current_time = ros::Time::now();
409  std::string odom_frame =
411  std::string base_footprint_frame =
413 
414  // TODO create some non-perfect odometry!
415  // getting data for base_footprint to odom transform
416 #if GAZEBO_MAJOR_VERSION >= 8
417  ignition::math::Pose3d pose = this->parent->WorldPose();
418 #else
419  ignition::math::Pose3d pose = this->parent->GetWorldPose().Ign();
420 #endif
421 
422  tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
423  tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
424 
425  tf::Transform base_footprint_to_odom(qt, vt);
426  if (this->broadcast_tf_) {
427 
429  tf::StampedTransform(base_footprint_to_odom, current_time,
430  odom_frame, base_footprint_frame));
431 
432  }
433 
434  // publish odom topic
435  odom_.pose.pose.position.x = pose.Pos().X();
436  odom_.pose.pose.position.y = pose.Pos().Y();
437 
438  odom_.pose.pose.orientation.x = pose.Rot().X();
439  odom_.pose.pose.orientation.y = pose.Rot().Y();
440  odom_.pose.pose.orientation.z = pose.Rot().Z();
441  odom_.pose.pose.orientation.w = pose.Rot().W();
442  odom_.pose.covariance[0] = this->covariance_x_;
443  odom_.pose.covariance[7] = this->covariance_y_;
444  odom_.pose.covariance[14] = 1000000000000.0;
445  odom_.pose.covariance[21] = 1000000000000.0;
446  odom_.pose.covariance[28] = 1000000000000.0;
447  odom_.pose.covariance[35] = this->covariance_yaw_;
448 
449  // get velocity in /odom frame
450  ignition::math::Vector3d linear;
451 #if GAZEBO_MAJOR_VERSION >= 8
452  linear = this->parent->WorldLinearVel();
453  odom_.twist.twist.angular.z = this->parent->WorldAngularVel().Z();
454 #else
455  linear = this->parent->GetWorldLinearVel().Ign();
456  odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().Ign().Z();
457 #endif
458 
459  // convert velocity to child_frame_id (aka base_footprint)
460  float yaw = pose.Rot().Yaw();
461  odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y();
462  odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X();
463  odom_.twist.covariance[0] = this->covariance_x_;
464  odom_.twist.covariance[7] = this->covariance_y_;
465  odom_.twist.covariance[14] = 1000000000000.0;
466  odom_.twist.covariance[21] = 1000000000000.0;
467  odom_.twist.covariance[28] = 1000000000000.0;
468  odom_.twist.covariance[35] = this->covariance_yaw_;
469 
470  odom_.header.stamp = current_time;
471  odom_.header.frame_id = odom_frame;
472  odom_.child_frame_id = base_footprint_frame;
473 
475  }
476 
478 }
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
#define ROS_WARN_NAMED(name,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
std::string resolve(const std::string &prefix, const std::string &frame_name)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
#define ROS_FATAL_STREAM_NAMED(name, args)
tf::TransformBroadcaster * transform_broadcaster_
void sendTransform(const StampedTransform &transform)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static Time now()
bool ok() const
boost::shared_ptr< void > VoidPtr


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27