diffdrive_plugin_multi_wheel.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 
47 /*
48  Copyright (c) 2014, Stefan Kohlbrecher
49  All rights reserved.
50 
51  Redistribution and use in source and binary forms, with or without
52  modification, are permitted provided that the following conditions are met:
53  * Redistributions of source code must retain the above copyright
54  notice, this list of conditions and the following disclaimer.
55  * Redistributions in binary form must reproduce the above copyright
56  notice, this list of conditions and the following disclaimer in the
57  documentation and/or other materials provided with the distribution.
58  * Neither the name of the <organization> nor the
59  names of its contributors may be used to endorse or promote products
60  derived from this software without specific prior written permission.
61 
62  THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
63  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
64  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
65  DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
66  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
67  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
68  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
69  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
70  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
71  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
72 
73 */
74 
75 #include <algorithm>
76 #include <assert.h>
77 
79 
80 #if (GAZEBO_MAJOR_VERSION < 8)
81 #include <gazebo/math/gzmath.hh>
82 #endif
83 #include <sdf/sdf.hh>
84 
85 #include <ros/ros.h>
87 #include <tf/transform_listener.h>
88 #include <geometry_msgs/Twist.h>
89 #include <nav_msgs/GetMap.h>
90 #include <nav_msgs/Odometry.h>
91 #include <boost/bind.hpp>
92 #include <boost/thread/mutex.hpp>
93 #include <boost/algorithm/string/split.hpp>
94 #include <boost/algorithm/string/classification.hpp>
95 
96 #include <gazebo/gazebo_config.h>
97 
98 namespace gazebo {
99 
100  enum {
103  };
104 
106 
107  // Destructor
109  delete rosnode_;
110  delete transform_broadcaster_;
111  }
112 
113  // Load the controller
114  void GazeboRosDiffDriveMultiWheel::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
115 
116  this->parent = _parent;
117  this->world = _parent->GetWorld();
118 
119  this->robot_namespace_ = "";
120  if (!_sdf->HasElement("robotNamespace")) {
121  ROS_INFO("GazeboRosDiffDriveMultiWheel Plugin missing <robotNamespace>, defaults to \"%s\"",
122  this->robot_namespace_.c_str());
123  } else {
124  this->robot_namespace_ =
125  _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
126  }
127 
128  //this->left_joint_names_ = "left_joint";
129  if (!_sdf->HasElement("leftJoints")) {
130  gzthrow("Have to specify space separated left side joint names via <leftJoints> tag!");
131  } else {
132  std::string joint_string = _sdf->GetElement("leftJoints")->Get<std::string>();
133  boost::split( joint_names_[LEFT], joint_string, boost::is_any_of(" ") );
134  }
135 
136  //this->right_joint_names_ = "right_joint";
137  if (!_sdf->HasElement("rightJoints")) {
138  gzthrow("Have to specify space separated right side joint names via <rightJoints> tag!");
139  } else {
140  std::string joint_string = _sdf->GetElement("rightJoints")->Get<std::string>();
141  boost::split( joint_names_[RIGHT], joint_string, boost::is_any_of(" ") );
142  }
143 
144  this->wheel_separation_ = 0.34;
145  if (!_sdf->HasElement("wheelSeparation")) {
146  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <wheelSeparation>, defaults to %f",
147  this->robot_namespace_.c_str(), this->wheel_separation_);
148  } else {
149  this->wheel_separation_ =
150  _sdf->GetElement("wheelSeparation")->Get<double>();
151  }
152 
153  this->wheel_diameter_ = 0.15;
154  if (!_sdf->HasElement("wheelDiameter")) {
155  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <wheelDiameter>, defaults to %f",
156  this->robot_namespace_.c_str(), this->wheel_diameter_);
157  } else {
158  this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get<double>();
159  }
160 
161  this->torque = 5.0;
162  if (!_sdf->HasElement("torque")) {
163  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <torque>, defaults to %f",
164  this->robot_namespace_.c_str(), this->torque);
165  } else {
166  this->torque = _sdf->GetElement("torque")->Get<double>();
167  }
168 
169  this->command_topic_ = "cmd_vel";
170  if (!_sdf->HasElement("commandTopic")) {
171  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <commandTopic>, defaults to \"%s\"",
172  this->robot_namespace_.c_str(), this->command_topic_.c_str());
173  } else {
174  this->command_topic_ = _sdf->GetElement("commandTopic")->Get<std::string>();
175  }
176 
177  this->odometry_topic_ = "odom";
178  if (!_sdf->HasElement("odometryTopic")) {
179  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <odometryTopic>, defaults to \"%s\"",
180  this->robot_namespace_.c_str(), this->odometry_topic_.c_str());
181  } else {
182  this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get<std::string>();
183  }
184 
185  this->odometry_frame_ = "odom";
186  if (!_sdf->HasElement("odometryFrame")) {
187  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <odometryFrame>, defaults to \"%s\"",
188  this->robot_namespace_.c_str(), this->odometry_frame_.c_str());
189  } else {
190  this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get<std::string>();
191  }
192 
193  this->robot_base_frame_ = "base_footprint";
194  if (!_sdf->HasElement("robotBaseFrame")) {
195  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <robotBaseFrame>, defaults to \"%s\"",
196  this->robot_namespace_.c_str(), this->robot_base_frame_.c_str());
197  } else {
198  this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get<std::string>();
199  }
200 
201  this->update_rate_ = 100.0;
202  if (!_sdf->HasElement("updateRate")) {
203  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <updateRate>, defaults to %f",
204  this->robot_namespace_.c_str(), this->update_rate_);
205  } else {
206  this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
207  }
208 
209 
210  this->publish_odometry_tf_ = true;
211  if (!_sdf->HasElement("publishOdometryTf")) {
212  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <publishOdometryTf>, defaults to %s",
213  this->robot_namespace_.c_str(), this->publish_odometry_tf_ ? "true" : "false");
214  } else {
215  this->publish_odometry_tf_ = _sdf->GetElement("publishOdometryTf")->Get<bool>();
216  }
217 
218  this->publish_odometry_msg_ = true;
219  if (!_sdf->HasElement("publishOdometryMsg")) {
220  ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <publishOdometryMsg>, defaults to %s",
221  this->robot_namespace_.c_str(), this->publish_odometry_msg_ ? "true" : "false");
222  } else {
223  this->publish_odometry_msg_ = _sdf->GetElement("publishOdometryMsg")->Get<bool>();
224  }
225 
226 
227 
228  // Initialize update rate stuff
229  if (this->update_rate_ > 0.0) {
230  this->update_period_ = 1.0 / this->update_rate_;
231  } else {
232  this->update_period_ = 0.0;
233  }
234 #if (GAZEBO_MAJOR_VERSION >= 8)
235  last_update_time_ = this->world->SimTime();
236 #else
237  last_update_time_ = this->world->GetSimTime();
238 #endif
239 
240  // Initialize velocity stuff
241  wheel_speed_[RIGHT] = 0;
242  wheel_speed_[LEFT] = 0;
243 
244  x_ = 0;
245  rot_ = 0;
246  alive_ = true;
247 
248  for (size_t side = 0; side < 2; ++side){
249  for (size_t i = 0; i < joint_names_[side].size(); ++i){
250  joints_[side].push_back(this->parent->GetJoint(joint_names_[side][i]));
251  if (!joints_[side][i]){
252  char error[200];
253  snprintf(error, 200,
254  "GazeboRosDiffDriveMultiWheel Plugin (ns = %s) couldn't get hinge joint named \"%s\"",
255  this->robot_namespace_.c_str(), joint_names_[side][i].c_str());
256  gzthrow(error);
257  }
258 #if (GAZEBO_MAJOR_VERSION > 4)
259  joints_[side][i]->SetEffortLimit(0, torque);
260 #else
261  joints_[side][i]->SetMaxForce(0, torque);
262 #endif
263  }
264  }
265 
266  // Make sure the ROS node for Gazebo has already been initialized
267  if (!ros::isInitialized())
268  {
269  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
270  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
271  return;
272  }
273 
275 
276  ROS_INFO("Starting GazeboRosDiffDriveMultiWheel Plugin (ns = %s)!", this->robot_namespace_.c_str());
277 
280 
281  // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
283  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
284  boost::bind(&GazeboRosDiffDriveMultiWheel::cmdVelCallback, this, _1),
285  ros::VoidPtr(), &queue_);
286 
288 
289  odometry_publisher_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
290 
291  // start custom queue for diff drive
292  this->callback_queue_thread_ =
293  boost::thread(boost::bind(&GazeboRosDiffDriveMultiWheel::QueueThread, this));
294 
295  // listen to the update event (broadcast every simulation iteration)
296  this->update_connection_ =
297  event::Events::ConnectWorldUpdateBegin(
298  boost::bind(&GazeboRosDiffDriveMultiWheel::UpdateChild, this));
299 
300  }
301 
302  // Update the controller
304 #if (GAZEBO_MAJOR_VERSION >= 8)
305  common::Time current_time = this->world->SimTime();
306 #else
307  common::Time current_time = this->world->GetSimTime();
308 #endif
309  double seconds_since_last_update =
310  (current_time - last_update_time_).Double();
311  if (seconds_since_last_update > update_period_) {
312 
313  if (this->publish_odometry_tf_ || this->publish_odometry_msg_){
314  publishOdometry(seconds_since_last_update);
315  }
316 
317  // Update robot in case new velocities have been requested
319  //joints[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / wheel_diameter_);
320  //joints[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / wheel_diameter_);
321 
322  for (size_t side = 0; side < 2; ++side){
323  for (size_t i = 0; i < joints_[side].size(); ++i){
324  joints_[side][i]->SetVelocity(0, wheel_speed_[side] / (0.5 * wheel_diameter_));
325  }
326  }
327 
328  last_update_time_+= common::Time(update_period_);
329 
330  }
331  }
332 
333  // Finalize the controller
335  alive_ = false;
336  queue_.clear();
337  queue_.disable();
338  rosnode_->shutdown();
339  callback_queue_thread_.join();
340  }
341 
343  boost::mutex::scoped_lock scoped_lock(lock);
344 
345  double vr = x_;
346  double va = rot_;
347 
348  wheel_speed_[LEFT] = vr - va * wheel_separation_ / 2.0;
349  wheel_speed_[RIGHT] = vr + va * wheel_separation_ / 2.0;
350  }
351 
353  const geometry_msgs::Twist::ConstPtr& cmd_msg) {
354 
355  boost::mutex::scoped_lock scoped_lock(lock);
356  x_ = cmd_msg->linear.x;
357  rot_ = cmd_msg->angular.z;
358  }
359 
361  static const double timeout = 0.01;
362 
363  while (alive_ && rosnode_->ok()) {
365  }
366  }
367 
369  ros::Time current_time = ros::Time::now();
370  std::string odom_frame =
372  std::string base_footprint_frame =
374 
375  // getting data for base_footprint to odom transform
376 #if (GAZEBO_MAJOR_VERSION >= 8)
377  ignition::math::Pose3d pose = this->parent->WorldPose();
378 
379  tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
380  tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
381 #else
382  math::Pose pose = this->parent->GetWorldPose();
383 
384  tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
385  tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
386 #endif
387 
388  tf::Transform base_footprint_to_odom(qt, vt);
389 
390  if (this->publish_odometry_tf_){
392  tf::StampedTransform(base_footprint_to_odom, current_time,
393  odom_frame, base_footprint_frame));
394  }
395 
396  // publish odom topic
397 #if (GAZEBO_MAJOR_VERSION >= 8)
398  odom_.pose.pose.position.x = pose.Pos().X();
399  odom_.pose.pose.position.y = pose.Pos().Y();
400 
401  odom_.pose.pose.orientation.x = pose.Rot().X();
402  odom_.pose.pose.orientation.y = pose.Rot().Y();
403  odom_.pose.pose.orientation.z = pose.Rot().Z();
404  odom_.pose.pose.orientation.w = pose.Rot().W();
405 #else
406  odom_.pose.pose.position.x = pose.pos.x;
407  odom_.pose.pose.position.y = pose.pos.y;
408 
409  odom_.pose.pose.orientation.x = pose.rot.x;
410  odom_.pose.pose.orientation.y = pose.rot.y;
411  odom_.pose.pose.orientation.z = pose.rot.z;
412  odom_.pose.pose.orientation.w = pose.rot.w;
413 #endif
414  odom_.pose.covariance[0] = 0.00001;
415  odom_.pose.covariance[7] = 0.00001;
416  odom_.pose.covariance[14] = 1000000000000.0;
417  odom_.pose.covariance[21] = 1000000000000.0;
418  odom_.pose.covariance[28] = 1000000000000.0;
419  odom_.pose.covariance[35] = 0.001;
420 
421  // get velocity in /odom frame
422 #if (GAZEBO_MAJOR_VERSION >= 8)
423  ignition::math::Vector3d linear;
424  linear = this->parent->WorldLinearVel();
425  odom_.twist.twist.angular.z = this->parent->WorldAngularVel().Z();
426 #else
427  math::Vector3 linear;
428  linear = this->parent->GetWorldLinearVel();
429  odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z;
430 #endif
431 
432  // convert velocity to child_frame_id (aka base_footprint)
433 #if (GAZEBO_MAJOR_VERSION >= 8)
434  float yaw = pose.Rot().Yaw();
435  odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y();
436  odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X();
437 #else
438  float yaw = pose.rot.GetYaw();
439  odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y;
440  odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x;
441 #endif
442 
443  odom_.header.stamp = current_time;
444  odom_.header.frame_id = odom_frame;
445  odom_.child_frame_id = base_footprint_frame;
446 
447  if (this->publish_odometry_msg_){
449  }
450  }
451 
452  GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffDriveMultiWheel)
453 }
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
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 cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
#define ROS_WARN(...)
std::string resolve(const std::string &prefix, const std::string &frame_name)
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
std::vector< physics::JointPtr > joints_[2]
#define ROS_FATAL_STREAM(args)
#define ROS_INFO(...)
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


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30