gazebo_ros_tricycle_drive.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
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 copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *********************************************************************/
33 
42 #include <algorithm>
43 #include <assert.h>
44 
46 
47 #include <ignition/math/Pose3.hh>
48 #include <ignition/math/Vector3.hh>
49 #include <sdf/sdf.hh>
50 
51 #include <ros/ros.h>
53 #include <tf/transform_listener.h>
54 #include <geometry_msgs/Twist.h>
55 #include <nav_msgs/GetMap.h>
56 #include <nav_msgs/Odometry.h>
57 #include <boost/bind.hpp>
58 #include <boost/thread/mutex.hpp>
59 
60 namespace gazebo
61 {
62 
63 enum {
64  RIGHT,
65  LEFT,
66 };
67 
69 
70 // Destructor
72 
73 // Load the controller
74 void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
75 {
76  parent = _parent;
77  gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "TricycleDrive" ) );
78  // Make sure the ROS node for Gazebo has already been initialized
79  gazebo_ros_->isInitialized();
80 
81  gazebo_ros_->getParameter<double> ( diameter_actuated_wheel_, "actuatedWheelDiameter", 0.15 );
82  gazebo_ros_->getParameter<double> ( diameter_encoder_wheel_, "encoderWheelDiameter", 0.15 );
83  gazebo_ros_->getParameter<double> ( wheel_torque_, "wheelTorque", 5.0 );
84  gazebo_ros_->getParameter<std::string> ( command_topic_, "commandTopic", "cmd_vel" );
85  gazebo_ros_->getParameter<std::string> ( odometry_topic_, "odometryTopic", "odom" );
86  gazebo_ros_->getParameter<std::string> ( odometry_frame_, "odometryFrame", "odom" );
87  gazebo_ros_->getParameter<std::string> ( robot_base_frame_, "robotBaseFrame", "base_link" );
88 
89  gazebo_ros_->getParameter<double> ( update_rate_, "updateRate", 100.0 );
90  gazebo_ros_->getParameter<double> ( wheel_acceleration_, "wheelAcceleration", 0 );
91  gazebo_ros_->getParameter<double> ( wheel_deceleration_, "wheelDeceleration", wheel_acceleration_ );
92  gazebo_ros_->getParameter<double> ( wheel_speed_tolerance_, "wheelSpeedTolerance", 0.01 );
93  gazebo_ros_->getParameter<double> ( steering_speed_, "steeringSpeed", 0 );
94  gazebo_ros_->getParameter<double> ( steering_angle_tolerance_, "steeringAngleTolerance", 0.01 );
95  gazebo_ros_->getParameter<double> ( separation_encoder_wheel_, "encoderWheelSeparation", 0.5 );
96 
97  gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false );
98  gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false );
99 
100  gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false );
101 
102  joint_steering_ = gazebo_ros_->getJoint ( parent, "steeringJoint", "front_steering_joint" );
103  joint_wheel_actuated_ = gazebo_ros_->getJoint ( parent, "actuatedWheelJoint", "front_wheel_joint" );
104  joint_wheel_encoder_left_ = gazebo_ros_->getJoint ( parent, "encoderWheelLeftJoint", "left_wheel_joint" );
105  joint_wheel_encoder_right_ = gazebo_ros_->getJoint ( parent, "encoderWheelRightJoint", "right_wheel_joint" );
106 
107  std::map<std::string, OdomSource> odomOptions;
108  odomOptions["encoder"] = ENCODER;
109  odomOptions["world"] = WORLD;
110  gazebo_ros_->getParameter<OdomSource> ( odom_source_, "odometrySource", odomOptions, WORLD );
111 
112  joint_wheel_actuated_->SetParam ( "fmax", 0, wheel_torque_ );
113  joint_steering_->SetParam ( "fmax", 0, wheel_torque_ );
114 
115 
116  // Initialize update rate stuff
117  if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_;
118  else this->update_period_ = 0.0;
119 #if GAZEBO_MAJOR_VERSION >= 8
120  last_actuator_update_ = parent->GetWorld()->SimTime();
121 #else
122  last_actuator_update_ = parent->GetWorld()->GetSimTime();
123 #endif
124 
125  // Initialize velocity stuff
126  alive_ = true;
127 
128  if ( this->publishWheelJointState_ ) {
129  joint_state_publisher_ = gazebo_ros_->node()->advertise<sensor_msgs::JointState> ( "joint_states", 1000 );
130  ROS_INFO_NAMED("tricycle_drive", "%s: Advertise joint_states", gazebo_ros_->info() );
131  }
132 
134 
135  // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
136  ROS_INFO_NAMED("tricycle_drive", "%s: Try to subscribe to %s", gazebo_ros_->info(), command_topic_.c_str() );
137 
139  ros::SubscribeOptions::create<geometry_msgs::Twist> ( command_topic_, 1,
140  boost::bind ( &GazeboRosTricycleDrive::cmdVelCallback, this, _1 ),
141  ros::VoidPtr(), &queue_ );
142 
143  cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe ( so );
144  ROS_INFO_NAMED("tricycle_drive", "%s: Subscribe to %s", gazebo_ros_->info(), command_topic_.c_str() );
145 
146  odometry_publisher_ = gazebo_ros_->node()->advertise<nav_msgs::Odometry> ( odometry_topic_, 1 );
147  ROS_INFO_NAMED("tricycle_drive", "%s: Advertise odom on %s ", gazebo_ros_->info(), odometry_topic_.c_str() );
148 
149  // start custom queue for diff drive
150  this->callback_queue_thread_ = boost::thread ( boost::bind ( &GazeboRosTricycleDrive::QueueThread, this ) );
151 
152  // listen to the update event (broadcast every simulation iteration)
153  this->update_connection_ = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosTricycleDrive::UpdateChild, this ) );
154 
155 }
156 
158 {
159  std::vector<physics::JointPtr> joints;
160  joints.push_back ( joint_steering_ );
161  joints.push_back ( joint_wheel_actuated_ );
162  joints.push_back ( joint_wheel_encoder_left_ );
163  joints.push_back ( joint_wheel_encoder_right_ );
164 
165  ros::Time current_time = ros::Time::now();
166  joint_state_.header.stamp = current_time;
167  joint_state_.name.resize ( joints.size() );
168  joint_state_.position.resize ( joints.size() );
169  joint_state_.velocity.resize ( joints.size() );
170  joint_state_.effort.resize ( joints.size() );
171  for ( std::size_t i = 0; i < joints.size(); i++ ) {
172  joint_state_.name[i] = joints[i]->GetName();
173 #if GAZEBO_MAJOR_VERSION >= 8
174  joint_state_.position[i] = joints[i]->Position ( 0 );
175 #else
176  joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian();
177 #endif
178  joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 );
179  joint_state_.effort[i] = joints[i]->GetForce ( 0 );
180  }
182 }
183 
185 {
186  ros::Time current_time = ros::Time::now();
187  std::vector<physics::JointPtr> joints;
188  joints.push_back ( joint_steering_ );
189  joints.push_back ( joint_wheel_actuated_ );
190  joints.push_back ( joint_wheel_encoder_left_ );
191  joints.push_back ( joint_wheel_encoder_right_ );
192  for ( std::size_t i = 0; i < joints.size(); i++ ) {
193  std::string frame = gazebo_ros_->resolveTF ( joints[i]->GetName() );
194  std::string parent_frame = gazebo_ros_->resolveTF ( joints[i]->GetParent()->GetName() );
195 
196 #if GAZEBO_MAJOR_VERSION >= 8
197  ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose();
198 #else
199  ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign();
200 #endif
201 
202  tf::Quaternion qt ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
203  tf::Vector3 vt ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
204 
205  tf::Transform transform ( qt, vt );
206  transform_broadcaster_->sendTransform ( tf::StampedTransform ( transform, current_time, parent_frame, frame ) );
207  }
208 
209 }
210 // Update the controller
212 {
214 #if GAZEBO_MAJOR_VERSION >= 8
215  common::Time current_time = parent->GetWorld()->SimTime();
216 #else
217  common::Time current_time = parent->GetWorld()->GetSimTime();
218 #endif
219  double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double();
220  if ( seconds_since_last_update > update_period_ ) {
221 
222  publishOdometry ( seconds_since_last_update );
225 
226  double target_wheel_roation_speed = cmd_.speed / ( diameter_actuated_wheel_ / 2.0 );
227  double target_steering_angle = cmd_.angle;
228 
229  motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update );
230 
231  //ROS_INFO_NAMED("tricycle_drive", "v = %f, w = %f ", target_wheel_roation_speed, target_steering_angle);
232 
233  last_actuator_update_ += common::Time ( update_period_ );
234  }
235 }
236 
237 
238 void GazeboRosTricycleDrive::motorController ( double target_speed, double target_angle, double dt )
239 {
240  double applied_speed = target_speed;
241  double applied_angle = target_angle;
242 
243  double current_speed = joint_wheel_actuated_->GetVelocity ( 0 );
244  if (wheel_acceleration_ > 0)
245  {
246  double diff_speed = current_speed - target_speed;
247  if ( fabs ( diff_speed ) < wheel_speed_tolerance_ )
248  {
249  applied_speed = current_speed;
250  }
251  else if ( fabs(diff_speed) > wheel_acceleration_ * dt )
252  {
253  if(diff_speed > 0)
254  {
255  applied_speed = current_speed - wheel_acceleration_ * dt;
256  }
257  else
258  {
259  applied_speed = current_speed + wheel_deceleration_ * dt;
260  }
261  }
262  }
263  joint_wheel_actuated_->SetParam ( "vel", 0, applied_speed );
264 
265 #if GAZEBO_MAJOR_VERSION >= 8
266  double current_angle = joint_steering_->Position ( 0 );
267 #else
268  double current_angle = joint_steering_->GetAngle ( 0 ).Radian();
269 #endif
270 
271  // truncate target angle
272  if (target_angle > +M_PI / 2.0)
273  {
274  target_angle = +M_PI / 2.0;
275  }
276  else if (target_angle < -M_PI / 2.0)
277  {
278  target_angle = -M_PI / 2.0;
279  }
280 
281  // if steering_speed_ is > 0, use speed control, otherwise use position control
282  // With position control, one cannot expect dynamics to work correctly.
283  double diff_angle = current_angle - target_angle;
284  if ( steering_speed_ > 0 ) {
285  // this means we will steer using steering speed
286  double applied_steering_speed = 0;
287  if (fabs(diff_angle) < steering_angle_tolerance_ ) {
288  // we're withing angle tolerance
289  applied_steering_speed = 0;
290  } else if ( diff_angle < target_speed ) {
291  // steer toward target angle
292  applied_steering_speed = steering_speed_;
293  } else {
294  // steer toward target angle
295  applied_steering_speed = -steering_speed_;
296  }
297 
298  // use speed control, not recommended, for better dynamics use force control
299  joint_steering_->SetParam ( "vel", 0, applied_steering_speed );
300  }
301  else {
302  // steering_speed_ is zero, use position control.
303  // This is not a good idea if we want dynamics to work.
304  if (fabs(diff_angle) < steering_speed_ * dt)
305  {
306  // we can take a step and still not overshoot target
307  if(diff_angle > 0)
308  {
309  applied_angle = current_angle - steering_speed_ * dt;
310  }
311  else
312  {
313  applied_angle = current_angle + steering_speed_ * dt;
314  }
315  }
316  else
317  {
318  applied_angle = target_angle;
319  }
320 #if GAZEBO_MAJOR_VERSION >= 9
321  joint_steering_->SetPosition(0, applied_angle, true);
322 #else
323  joint_steering_->SetPosition(0, applied_angle);
324 #endif
325  }
326  //ROS_INFO_NAMED("tricycle_drive", "target: [%3.2f, %3.2f], current: [%3.2f, %3.2f], applied: [%3.2f, %3.2f/%3.2f] ",
327  // target_speed, target_angle, current_speed, current_angle, applied_speed, applied_angle, applied_steering_speed );
328 }
329 
330 // Finalize the controller
332 {
333  alive_ = false;
334  queue_.clear();
335  queue_.disable();
336  gazebo_ros_->node()->shutdown();
337  callback_queue_thread_.join();
338 }
339 
340 void GazeboRosTricycleDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg )
341 {
342  boost::mutex::scoped_lock scoped_lock ( lock );
343  cmd_.speed = cmd_msg->linear.x;
344  cmd_.angle = cmd_msg->angular.z;
345 }
346 
348 {
349  static const double timeout = 0.01;
350 
351  while ( alive_ && gazebo_ros_->node()->ok() ) {
352  queue_.callAvailable ( ros::WallDuration ( timeout ) );
353  }
354 }
355 
357 {
358  double vl = joint_wheel_encoder_left_->GetVelocity ( 0 );
359  double vr = joint_wheel_encoder_right_->GetVelocity ( 0 );
360 #if GAZEBO_MAJOR_VERSION >= 8
361  common::Time current_time = parent->GetWorld()->SimTime();
362 #else
363  common::Time current_time = parent->GetWorld()->GetSimTime();
364 #endif
365  double seconds_since_last_update = ( current_time - last_odom_update_ ).Double();
366  last_odom_update_ = current_time;
367 
368  double b = separation_encoder_wheel_;
369 
370  // Book: Sigwart 2011 Autonompus Mobile Robots page:337
371  double sl = vl * ( diameter_encoder_wheel_ / 2.0 ) * seconds_since_last_update;
372  double sr = vr * ( diameter_encoder_wheel_ / 2.0 ) * seconds_since_last_update;
373  double theta = ( sl - sr ) /b;
374 
375 
376  double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
377  double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
378  double dtheta = ( sl - sr ) /b;
379 
380  pose_encoder_.x += dx;
381  pose_encoder_.y += dy;
382  pose_encoder_.theta += dtheta;
383 
384  double w = dtheta/seconds_since_last_update;
385  double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;
386 
387  tf::Quaternion qt;
388  tf::Vector3 vt;
389  qt.setRPY ( 0,0,pose_encoder_.theta );
390  vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 );
391 
392  odom_.pose.pose.position.x = vt.x();
393  odom_.pose.pose.position.y = vt.y();
394  odom_.pose.pose.position.z = vt.z();
395 
396  odom_.pose.pose.orientation.x = qt.x();
397  odom_.pose.pose.orientation.y = qt.y();
398  odom_.pose.pose.orientation.z = qt.z();
399  odom_.pose.pose.orientation.w = qt.w();
400 
401  odom_.twist.twist.angular.z = w;
402  odom_.twist.twist.linear.x = dx/seconds_since_last_update;
403  odom_.twist.twist.linear.y = dy/seconds_since_last_update;
404 }
405 
407 {
408 
409  ros::Time current_time = ros::Time::now();
410  std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ );
411  std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ );
412 
413  tf::Quaternion qt;
414  tf::Vector3 vt;
415 
416  if ( odom_source_ == ENCODER ) {
417  // getting data form encoder integration
418  qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w );
419  vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z );
420 
421  }
422  if ( odom_source_ == WORLD ) {
423  // getting data form gazebo world
424 #if GAZEBO_MAJOR_VERSION >= 8
425  ignition::math::Pose3d pose = parent->WorldPose();
426 #else
427  ignition::math::Pose3d pose = parent->GetWorldPose().Ign();
428 #endif
429  qt = tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );
430  vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );
431 
432  odom_.pose.pose.position.x = vt.x();
433  odom_.pose.pose.position.y = vt.y();
434  odom_.pose.pose.position.z = vt.z();
435 
436  odom_.pose.pose.orientation.x = qt.x();
437  odom_.pose.pose.orientation.y = qt.y();
438  odom_.pose.pose.orientation.z = qt.z();
439  odom_.pose.pose.orientation.w = qt.w();
440 
441  // get velocity in /odom frame
442  ignition::math::Vector3d linear;
443 #if GAZEBO_MAJOR_VERSION >= 8
444  linear = parent->WorldLinearVel();
445  odom_.twist.twist.angular.z = parent->WorldAngularVel().Z();
446 #else
447  linear = parent->GetWorldLinearVel().Ign();
448  odom_.twist.twist.angular.z = parent->GetWorldAngularVel().Ign().Z();
449 #endif
450 
451  // convert velocity to child_frame_id (aka base_footprint)
452  float yaw = pose.Rot().Yaw();
453  odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y();
454  odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X();
455  }
456 
457  tf::Transform base_footprint_to_odom ( qt, vt );
458  transform_broadcaster_->sendTransform (
459  tf::StampedTransform ( base_footprint_to_odom, current_time,
460  odom_frame, base_footprint_frame ) );
461 
462 
463  // set covariance
464  odom_.pose.covariance[0] = 0.00001;
465  odom_.pose.covariance[7] = 0.00001;
466  odom_.pose.covariance[14] = 1000000000000.0;
467  odom_.pose.covariance[21] = 1000000000000.0;
468  odom_.pose.covariance[28] = 1000000000000.0;
469  odom_.pose.covariance[35] = 0.001;
470 
471 
472  // set header
473  odom_.header.stamp = current_time;
474  odom_.header.frame_id = odom_frame;
475  odom_.child_frame_id = base_footprint_frame;
476 
478 }
479 
481 }
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
void UpdateOdometryEncoder()
updates the relative robot pose based on the wheel encoders
void publishWheelJointState()
publishes the wheel tf&#39;s
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
TFSIMD_FORCE_INLINE const tfScalar & x() const
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
static Time now()
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
void motorController(double target_speed, double target_angle, double dt)
boost::shared_ptr< void > VoidPtr
boost::shared_ptr< GazeboRos > GazeboRosPtr


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