diffdrive_plugin_6w.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
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 Simulation, Systems Optimization and Robotics
13 // group, TU Darmstadt nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
33 #include <algorithm>
34 #include <assert.h>
35 
37 #include <gazebo/common/Events.hh>
38 #include <gazebo/physics/physics.hh>
39 
40 #include <ros/ros.h>
42 #include <tf/transform_listener.h>
43 #include <geometry_msgs/Twist.h>
44 #include <nav_msgs/Odometry.h>
45 #include <boost/bind.hpp>
46 
47 #include <gazebo/gazebo_config.h>
48 
49 namespace gazebo {
50 
51 enum
52 {
60 };
61 
62 // Constructor
64 {
65 }
66 
67 // Destructor
69 {
70 #if (GAZEBO_MAJOR_VERSION >= 8)
71  updateConnection.reset();
72 #else
73  event::Events::DisconnectWorldUpdateBegin(updateConnection);
74 #endif
76  rosnode_->shutdown();
78  delete rosnode_;
79 }
80 
81 // Load the controller
82 void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
83 {
84  world = _model->GetWorld();
85 
86  // default parameters
87  namespace_.clear();
88  topic_ = "cmd_vel";
89  wheelSep = 0.34;
90  wheelDiam = 0.15;
91  torque = 10.0;
92 
93  // load parameters
94  if (_sdf->HasElement("robotNamespace"))
95  namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
96 
97  if (_sdf->HasElement("topicName"))
98  topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
99 
100  if (_sdf->HasElement("bodyName"))
101  {
102  link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
103  link = _model->GetLink(link_name_);
104  } else {
105  link = _model->GetLink();
106  link_name_ = link->GetName();
107  }
108 
109  // assert that the body by link_name_ exists
110  if (!link)
111  {
112  ROS_FATAL("DiffDrivePlugin6W plugin error: bodyName: %s does not exist\n", link_name_.c_str());
113  return;
114  }
115 
116  if (_sdf->HasElement("frontLeftJoint")) joints[FRONT_LEFT] = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->GetValue()->GetAsString());
117  if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->GetValue()->GetAsString());
118  if (_sdf->HasElement("midLeftJoint")) joints[MID_LEFT] = _model->GetJoint(_sdf->GetElement("midLeftJoint")->GetValue()->GetAsString());
119  if (_sdf->HasElement("midRightJoint")) joints[MID_RIGHT] = _model->GetJoint(_sdf->GetElement("midRightJoint")->GetValue()->GetAsString());
120  if (_sdf->HasElement("rearLeftJoint")) joints[REAR_LEFT] = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->GetValue()->GetAsString());
121  if (_sdf->HasElement("rearRightJoint")) joints[REAR_RIGHT] = _model->GetJoint(_sdf->GetElement("rearRightJoint")->GetValue()->GetAsString());
122 
123  if (!joints[FRONT_LEFT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front left joint");
124  if (!joints[FRONT_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front right joint");
125  if (!joints[MID_LEFT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid left joint");
126  if (!joints[MID_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid right joint");
127  if (!joints[REAR_LEFT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear left joint");
128  if (!joints[REAR_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear right joint");
129 
130  if (_sdf->HasElement("wheelSeparation"))
131  _sdf->GetElement("wheelSeparation")->GetValue()->Get(wheelSep);
132 
133  if (_sdf->HasElement("wheelDiameter"))
134  _sdf->GetElement("wheelDiameter")->GetValue()->Get(wheelDiam);
135 
136  if (_sdf->HasElement("torque"))
137  _sdf->GetElement("torque")->GetValue()->Get(torque);
138 
139  // Make sure the ROS node for Gazebo has already been initialized
140  if (!ros::isInitialized())
141  {
142  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
143  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
144  return;
145  }
146 
148 
151 
152  // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
154  ros::SubscribeOptions::create<geometry_msgs::Twist>(topic_, 1,
155  boost::bind(&DiffDrivePlugin6W::cmdVelCallback, this, _1),
156  ros::VoidPtr(), &queue_);
157  sub_ = rosnode_->subscribe(so);
158  pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);
159 
160  callback_queue_thread_ = boost::thread(boost::bind(&DiffDrivePlugin6W::QueueThread, this));
161 
162  Reset();
163 
164  // New Mechanism for Updating every World Cycle
165  // Listen to the update event. This event is broadcast every
166  // simulation iteration.
167  updateConnection = event::Events::ConnectWorldUpdateBegin(
168  boost::bind(&DiffDrivePlugin6W::Update, this));
169 }
170 
171 // Initialize the controller
173 {
174  enableMotors = true;
175 
176  for (size_t i = 0; i < 2; ++i){
177  wheelSpeed[i] = 0;
178  }
179 
180 #if (GAZEBO_MAJOR_VERSION >= 8)
181  prevUpdateTime = world->SimTime();
182 #else
183  prevUpdateTime = world->GetSimTime();
184 #endif
185 
186  x_ = 0;
187  rot_ = 0;
188  alive_ = true;
189 
190  // Reset odometric pose
191  odomPose[0] = 0.0;
192  odomPose[1] = 0.0;
193  odomPose[2] = 0.0;
194 
195  odomVel[0] = 0.0;
196  odomVel[1] = 0.0;
197  odomVel[2] = 0.0;
198 }
199 
200 // Update the controller
202 {
203  // TODO: Step should be in a parameter of this function
204  double d1, d2;
205  double dr, da;
206  common::Time stepTime;
207 
208  GetPositionCmd();
209 
210  //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
211 #if (GAZEBO_MAJOR_VERSION >= 8)
212  stepTime = world->SimTime() - prevUpdateTime;
213  prevUpdateTime = world->SimTime();
214 #else
215  stepTime = world->GetSimTime() - prevUpdateTime;
216  prevUpdateTime = world->GetSimTime();
217 #endif
218 
219  // Distance travelled by front wheels
220  d1 = stepTime.Double() * wheelDiam / 2 * joints[MID_LEFT]->GetVelocity(0);
221  d2 = stepTime.Double() * wheelDiam / 2 * joints[MID_RIGHT]->GetVelocity(0);
222 
223  dr = (d1 + d2) / 2;
224  da = (d1 - d2) / wheelSep;
225 
226  // Compute odometric pose
227  odomPose[0] += dr * cos(odomPose[2]);
228  odomPose[1] += dr * sin(odomPose[2]);
229  odomPose[2] += da;
230 
231  // Compute odometric instantaneous velocity
232  odomVel[0] = dr / stepTime.Double();
233  odomVel[1] = 0.0;
234  odomVel[2] = da / stepTime.Double();
235 
236  if (enableMotors)
237  {
238  joints[FRONT_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
239  joints[MID_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
240  joints[REAR_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0));
241 
242  joints[FRONT_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
243  joints[MID_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
244  joints[REAR_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0));
245 
246 #if (GAZEBO_MAJOR_VERSION > 4)
247  joints[FRONT_LEFT]->SetEffortLimit(0, torque);
248  joints[MID_LEFT]->SetEffortLimit(0, torque);
249  joints[REAR_LEFT]->SetEffortLimit(0, torque);
250 
251  joints[FRONT_RIGHT]->SetEffortLimit(0, torque);
252  joints[MID_RIGHT]->SetEffortLimit(0, torque);
253  joints[REAR_RIGHT]->SetEffortLimit(0, torque);
254 #else
255  joints[FRONT_LEFT]->SetMaxForce(0, torque);
256  joints[MID_LEFT]->SetMaxForce(0, torque);
257  joints[REAR_LEFT]->SetMaxForce(0, torque);
258 
259  joints[FRONT_RIGHT]->SetMaxForce(0, torque);
260  joints[MID_RIGHT]->SetMaxForce(0, torque);
261  joints[REAR_RIGHT]->SetMaxForce(0, torque);
262 #endif
263  }
264 
265  //publish_odometry();
266 }
267 
268 // NEW: Now uses the target velocities from the ROS message, not the Iface
270 {
271  lock.lock();
272 
273  double vr, va;
274 
275  vr = x_; //myIface->data->cmdVelocity.pos.x;
276  va = -rot_; //myIface->data->cmdVelocity.yaw;
277 
278  //std::cout << "X: [" << x_ << "] ROT: [" << rot_ << "]" << std::endl;
279 
280  // Changed motors to be always on, which is probably what we want anyway
281  enableMotors = true; //myIface->data->cmdEnableMotors > 0;
282 
283  //std::cout << enableMotors << std::endl;
284 
285  wheelSpeed[0] = vr + va * wheelSep / 2;
286  wheelSpeed[1] = vr - va * wheelSep / 2;
287 
288  lock.unlock();
289 }
290 
291 // NEW: Store the velocities from the ROS message
292 void DiffDrivePlugin6W::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
293 {
294  //std::cout << "BEGIN CALLBACK\n";
295 
296  lock.lock();
297 
298  x_ = cmd_msg->linear.x;
299  rot_ = cmd_msg->angular.z;
300 
301  lock.unlock();
302 
303  //std::cout << "END CALLBACK\n";
304 }
305 
306 // NEW: custom callback queue thread
308 {
309  static const double timeout = 0.01;
310 
311  while (alive_ && rosnode_->ok())
312  {
313  // std::cout << "CALLING STUFF\n";
315  }
316 }
317 
318 // NEW: Update this to publish odometry topic
320 {
321  // get current time
322 #if (GAZEBO_MAJOR_VERSION >= 8)
323  ros::Time current_time_((world->SimTime()).sec, (world->SimTime()).nsec);
324 #else
325  ros::Time current_time_((world->GetSimTime()).sec, (world->GetSimTime()).nsec);
326 #endif
327 
328  // getting data for base_footprint to odom transform
329 #if (GAZEBO_MAJOR_VERSION >= 8)
330  ignition::math::Pose3d pose = link->WorldPose();
331  ignition::math::Vector3d velocity = link->WorldLinearVel();
332  ignition::math::Vector3d angular_velocity = link->WorldAngularVel();
333 
334  tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
335  tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
336 #else
337  math::Pose pose = link->GetWorldPose();
338  math::Vector3 velocity = link->GetWorldLinearVel();
339  math::Vector3 angular_velocity = link->GetWorldAngularVel();
340 
341  tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
342  tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
343 #endif
344  tf::Transform base_footprint_to_odom(qt, vt);
345 
347  current_time_,
348  "odom",
349  "base_footprint"));
350 
351  // publish odom topic
352 #if (GAZEBO_MAJOR_VERSION >= 8)
353  odom_.pose.pose.position.x = pose.Pos().X();
354  odom_.pose.pose.position.y = pose.Pos().Y();
355 
356  odom_.pose.pose.orientation.x = pose.Rot().X();
357  odom_.pose.pose.orientation.y = pose.Rot().Y();
358  odom_.pose.pose.orientation.z = pose.Rot().Z();
359  odom_.pose.pose.orientation.w = pose.Rot().W();
360 
361  odom_.twist.twist.linear.x = velocity.X();
362  odom_.twist.twist.linear.y = velocity.Y();
363  odom_.twist.twist.angular.z = angular_velocity.Z();
364 #else
365  odom_.pose.pose.position.x = pose.pos.x;
366  odom_.pose.pose.position.y = pose.pos.y;
367 
368  odom_.pose.pose.orientation.x = pose.rot.x;
369  odom_.pose.pose.orientation.y = pose.rot.y;
370  odom_.pose.pose.orientation.z = pose.rot.z;
371  odom_.pose.pose.orientation.w = pose.rot.w;
372 
373  odom_.twist.twist.linear.x = velocity.x;
374  odom_.twist.twist.linear.y = velocity.y;
375  odom_.twist.twist.angular.z = angular_velocity.z;
376 #endif
377 
378  odom_.header.frame_id = tf::resolve(tf_prefix_, "odom");
379  odom_.child_frame_id = "base_footprint";
380  odom_.header.stamp = current_time_;
381 
382  pub_.publish(odom_);
383 }
384 
385 GZ_REGISTER_MODEL_PLUGIN(DiffDrivePlugin6W)
386 
387 } // namespace gazebo
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
#define ROS_FATAL(...)
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()
physics::JointPtr joints[6]
std::string resolve(const std::string &prefix, const std::string &frame_name)
#define ROS_FATAL_STREAM(args)
void sendTransform(const StampedTransform &transform)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool ok() const
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
tf::TransformBroadcaster * transform_broadcaster_
boost::shared_ptr< void > VoidPtr
event::ConnectionPtr updateConnection


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