Using Gazebo to create the odometry

To obtain some insight of how Gazebo does that, we are going to take a sneak peek inside the diffdrive_plugin.cpp file. You can find it at https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp.

The Load function performs the function of registering the subscriber of the topic, and when a cmd_vel topic is received, the cmdVelCallback() function is executed to handle the message:

void GazeboRosSkidSteerDrive::Load(physics::ModelPtr _parent, 
sdf::ElementPtr _sdf) 
{ 
  ... 
  ... 
  // ROS: Subscribe to the velocity command topic (usually 
  "cmd_vel") 
  ros::SubscribeOptions so = 
  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_ 
  , 1, 
  boost::bind(&GazeboRosSkidSteerDrive::cmdVelCallback, this, _1), 
  ros::VoidPtr(), &queue_); 
  ... 
  ... 
} 

When a message arrives, the linear and angular velocities are stored in the internal variables to run operations later:

void GazeboRosSkidSteerDrive::cmdVelCallback( 
const geometry_msgs::Twist::ConstPtr& cmd_msg) 
{ 
  boost::mutex::scoped_lock scoped_lock(lock); 
  x_ = cmd_msg->linear.x; 
  rot_ = cmd_msg->angular.z; 
} 

The plugin estimates the velocity for each motor, using the formulas from the kinematic model of the robot, in the following manner:

void GazeboRosSkidSteerDrive::getWheelVelocities() { 
  boost::mutex::scoped_lock scoped_lock(lock); 
  double vr = x_; 
  double va = rot_; 
  wheel_speed_[RIGHT_FRONT] = vr + va * wheel_separation_ / 2.0; 
  wheel_speed_[RIGHT_REAR] = vr + va * wheel_separation_ / 2.0; 
  wheel_speed_[LEFT_FRONT] = vr - va * wheel_separation_ / 2.0; 
  wheel_speed_[LEFT_REAR] = vr - va * wheel_separation_ / 2.0; 
} 

And finally, it estimates the distance traversed by the robot using more formulas from the kinematic motion model of the robot. As you can see in the code, you must know the wheel diameter and the wheel separation of your robot:

// Update the controller 
void GazeboRosSkidSteerDrive::UpdateChild() 
{ 
  common::Time current_time = this->world->GetSimTime(); 
  double seconds_since_last_update = 
  (current_time - last_update_time_).Double(); 
  if (seconds_since_last_update > update_period_) 
  { 
    publishOdometry(seconds_since_last_update); 
    // Update robot in case new velocities have been requested 
    getWheelVelocities(); 
    joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / 
wheel_diameter_); joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] /
wheel_diameter_); joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] /
wheel_diameter_); joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] /
wheel_diameter_); last_update_time_+= common::Time(update_period_); } }

This is the way gazebo_ros_skid_steer_drive controls our simulated robot in Gazebo.

..................Content has been hidden....................

You can't read the all page of ebook, please click here login for view all page.
Reset
18.188.215.1