In this chapter, we will integrate Gazebo’s Differential Drive plugin in the existing robot model. With that, we will able to manually steer the robot by sending velocity commands to its wheels. Differential Drive plugin is simpler and understanding that is essential to get a grasp of useful ROS concepts.

Key takeaways from this chapter are:

• In general, we will see how to integrate a Gazebo plugin.
• Use teleop ROS node to send velocity commands to the robot model.
• Configure rviz to visualize robot movement in the odometry frame.
• Understand odom and cmd_vel topics – common to any differential drive.
• Implement a simple custom velocity commands pubslisher.

Source Code Prep

Copy src/kbot_description from previous chapter and keep working in this directory.

Integrating Differential Drive Plugin

Create a new xacro file kbot_description/urdf/kbot_gazebo_plugins.xacro and paste below code to add the plugin.

Above code is how you would add a Differential Drive Gazebo plugin to the robot model. Any plugin within <plugin> tags must be included under <gazebo> tags. filename is the gazebo_ros library name containing the plugin implementation.

A plugin may have its own parameters. Here left_wheel_joint and right_wheel_joint are the wheel joint names already defined in kbot.xacro. Similarly, values for <wheelSeparation> and <wheelDiameter> come from the main xacro file.

This plugin subscribes to the topic /kbot/base_controller/cmd_vel and publishes topic /kbot/base_controller/odom (more on these in the next sections). Note that /kbot and /base_controller are just namespaces to avoid confusion (and collision) with similar topics from other robots or other controllers on the same robot.

Finally, the TF frame for odometry is odom and base frame of the robot is, of course, base_footprint.

To make this plugin a part of the robot model, include plugin xacro file to the main xacro file kbot_description/urdf/kbot.xacro

Using teleop Node to Steer the Robot

ROS provides a package to steer a robot base using the keyboard. A node of package teleop_twist_keyboard publishes a topic called cmd_vel that represents velocity commands for the robot. We need to map this topic to correspondng topic subscribed by Differential Drive Plugin (i.e. publisher-subscriber binding).

Create a new package kbot_simple_control to for all the source code related to KBot control.

Create a new launch file kbot_simple_control/launch/kbot_control_teleop.launch and add below code.

Viewing TF Graph

Let’s launch rviz and Gazebo using below command in a terminal.

Ignore any warnings about odometry for a moment. In another terminal launch teleop_twist_keyboard node. Do not press any teleop control keys yet!

Open a third terminal (obviously after sourcing devel/setup.sh) and run below command.

Above command creates a file frames.pdf containing TF graph. It shows different co-ordinate frames associated with KBot and their relationship. Your graph should like the image shown below. Note that Gazebo is publishing odom frame. This is due to the Differential Drive plugin. Whereas, all other frames are published automatically by robot_state_publisher based on URDF descriptions.

Configuring Rviz

You may notice some errors in rviz Displays window and may not even see the robot model. Some configuration parameters must be tweaked to fix this.

First, set Displays -> Global Options -> Fixed Frame to odom frame instead of map. We do not yet have a map frame. For this chapter, odom is the fixed world frame. We will see how to use a map in the next chapters.

Next, add RobotModel visualization to Displays by clicking ‘add’ and selecting it. Your robot model should be now be visible in the map area. In a similar way, add TF visualization to see co-ordinate frames. You may want to enable only few interesting frames to avoid cluttering in the GUI.

Also, change Current View -> Target Frame to odom. This ensure that odom frame at the initial position of the robot, while base_footprint and robot model navigate the world.

Now is the time for some action. Go to the terminal where teleop_keyboard_twist was launched. Press key ‘i’ and see the robot move forward. Please a relevant key to stop it. Go back to rviz and Gazebo to review the current position of the robot.

To save some effort, you can save the current rviz configuration as kbot_simple_control/config/rviz_odom.config using File -> Save Config on rviz menu. You can apply this configuration (after launching both nodes) from File -> Open Config.

Temporarily Fixing the Robot Drift

In the previous section, you may have noticed a few strange things about the robot motion.

• The robot drifted a bit even before issuing any velocity commands i.e. odom and base_footprint frames are not aligned at the origin.
• The robot did not follow a straight path even after commanding it to do so.
• Wheels of the robot did not have smooth rotation (woobly perhaps!).

Such issues are mostly due to inadequate mass of the robot or incorrect inertia formulae. If your robot is drifting, it means too light. It’s time to get back to URDF and increase the mass by a bit. Usually, heavy base_link, left_wheel & right_wheel are good enough for a stable robot model. Tweak the mass (‘m’) parameter of box/cylinder/sphere_inertia macros in kbot_description/urdf/kbot.xacro and rerun the simulation. Much better rviz path of a heavy robot with the same teleop command is shown below.

Understanding Topics cmd_vel & odom

To recap the flow so far, first teleop_twist_keyboard publishes velocity commands on a topic called cmd_vel. This topic is subscribed by the differential drive Gazebo plugin. The plugin drives the robot model according to the received messages. As the robot moves, the plugin publishes odometry information to odom topic.

But, what exactly constitues a velocity command or what is the format of published odometry message or what is odometry after all? These are the questions we try to answer in this section. Understanding these topics will help us implement custom velocity command and odometry publishers in Part-2 of this book. Immediately, this knowledge will be useful to implement a simple custom commands publisher (in the next section).

Twist

In a differential drive robot (and most wheeled robots), the wheel velocities determine the robot’s speed and orientation. However, robot’s (base) velocity is expressed as a single entity called Twist. The differential drive controller (here a plugin) breaks the Twist down to individual wheel velocities and the downstream hardware components actuate the wheels accordingly.

Twist has two components – linear velocity & angular velocity. Each of them is a velocity in the 3 dimension space (hence a vector). Unsurprisingly, the geometry_msgs/Twist message of cmd_vel topic is defined in the same way.

So, linear.x, linear.y & linear.z are linear velocities in x, y & z dimensions. And, angular.x, angular.y & angular.z are rotations in respective dimensions.

While the teleop node is running, do tfecho to see the published velocity commands.

Odometry

Odometry, as precisely defined by Wikipedia, is the use of data from motion sensors to estimate the change in position of a robot over time. This estimate is always relative to a fixed starting point in the robot’s environment. For our robot (so far), this fixed reference frame is odom. For robots navigating autonomously, it is usally a simulated or real world map.

Two things to note in the definition of Odometry:

• Odometry is a change in position over time. At any given time, Odometry tells how much the robot has moved from a previous location and orientation.
• Odometry is an estimation. Hence, a variance value is also usually computed to indicate how ‘off’ it is.

In ROS, odometry topic uses nav_msgs/Odometry message. For KBot frame_id is that odom while child_frame_id is that of base_footprint. The message also has Pose (more important) and Twist information. Pose (with covariance) is an estimation of position and orientation of the robot in 3 dimension space. The message also includes a time stamp (remember, estimation over time?).

Note that orientation is presented as a Quaternion (4 values). For now, think of Quaternion as a 4-element vector thats the result of encoding a 3-element rotation vector.

Simple Velocity Commands Publisher

Instead of using the built-in teleop package, we will implement a custom velocity commands publisher in Python that will send simple navigation goals to KBot.

Create a new file kbot_simple_control/src/kbot_simple_twist_pub.py

In one terminal,

In another terminal, run the Python script.

Above code defines a node called kbot_simple_twist_pub that publishes a Twist message directly to /kbot/base_controller/cmd_vel topic subscribed by Gazebo. msg.linear.x = 0.1 causes the robot to move forward slowly. You may also try to rotate the robot in place by setting msg.linear.x=0 and msg.angular.z=6.