As you have seen in other examples with Gazebo, our robot moves in the simulated world just like a robot in the real world. We use a driver for our robot, diffdrive_plugin.

This driver publishes the odometry generated in the simulated world, so we do not need to write anything for Gazebo.

Execute the robot sample in Gazebo to see the odometry working. Type the following commands in the shell:

    $ roslaunch chapter5_tutorials gazebo_xacro.launch model:="'rospack find robot1_description'/urdf/robot1_base_04.xacro"
    $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
  

Then, with the teleop node, move the robot for a few seconds to generate new data on the odometry topic.

On the screen of the Gazebo simulator, if you click on robot_model1, you will see some properties of the object. One of these properties is the pose of the robot. Click on the pose, and you will see some fields with data. What you are watching is the position of the robot in the virtual world. If you move the robot, the data changes:

Gazebo continuously publishes the odometry data. Check the topic and see what data it is sending. Type the following command in a shell:

    $ rostopic echo /odom/pose/pose
  

The following is the output that you will receive:

As you can observe, Gazebo is creating the odometry as the robot moves. We are going to see how Gazebo creates it by looking inside the plugin's source code.

The plugin file is located in the gazebo_plugins package, and the file is gazebo_ros_skid_steer_drive.cpp. You can find the code at https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp.

The file has a lot of code, but the important part for us now is the following function, publishOdometry():

void GazeboRosSkidSteerDrive::publishOdometry(double step_time) 
{ 
  ros::Time current_time = ros::Time::now(); 
  std::string odom_frame = 
  tf::resolve(tf_prefix_, odometry_frame_); 
  std::string base_footprint_frame = 
  tf::resolve(tf_prefix_, robot_base_frame_); 
  // TODO create some non-perfect odometry! 
  // getting data for base_footprint to odom transform 
  math::Pose pose = this->parent->GetWorldPose(); 
  tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); 
  tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); 
  tf::Transform base_footprint_to_odom(qt, vt); 
  if (this->broadcast_tf_) 
  { 
    transform_broadcaster_->sendTransform( 
    tf::StampedTransform(base_footprint_to_odom, current_time, 
    odom_frame, base_footprint_frame)); 
  } 
  // publish odom topic 
  odom_.pose.pose.position.x = pose.pos.x; 
  odom_.pose.pose.position.y = pose.pos.y; 
  odom_.pose.pose.orientation.x = pose.rot.x; 
  odom_.pose.pose.orientation.y = pose.rot.y; 
  odom_.pose.pose.orientation.z = pose.rot.z; 
  odom_.pose.pose.orientation.w = pose.rot.w; 
  odom_.pose.covariance[0] = 0.00001; 
  odom_.pose.covariance[7] = 0.00001; 
  odom_.pose.covariance[14] = 1000000000000.0; 
  odom_.pose.covariance[21] = 1000000000000.0; 
  odom_.pose.covariance[28] = 1000000000000.0; 
  odom_.pose.covariance[35] = 0.01; 
  // get velocity in /odom frame 
  math::Vector3 linear; 
  linear = this->parent->GetWorldLinearVel(); 
  odom_.twist.twist.angular.z = this->parent-> 
   GetWorldAngularVel().z; 
  // convert velocity to child_frame_id (aka base_footprint) 
  float yaw = pose.rot.GetYaw(); 
  odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * 
   linear.y;  odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * 
   linear.x; 
  odom_.header.stamp = current_time; 
  odom_.header.frame_id = odom_frame; 
  odom_.child_frame_id = base_footprint_frame; 
  odometry_publisher_.publish(odom_); 
} 

The publishOdometry() function is where the odometry is published. You can see how the fields of the structure are filled and the name of the topic for the odometry is set (in this case, it is odom). The pose is generated in the other part of the code that we will see in the following section.

Once you have learned how and where Gazebo creates the odometry, you will be ready to learn how to publish the odometry and the transform for a real robot. The following code will show a robot doing circles continuously. The final outcome does not really matter; the important thing to know here is how to publish the correct data for our robot.