Note: This is part of a series where I follow along with the ROS 2 Humble and Gazebo Fortress tutorials and demos. This is not original content.
Before you start working through today’s tutorial, make sure you’ve completed this one, where we built the robot we’ll be using below.
Last week’s post was based on this documentation, and today’s post will be based on these docs.
Ok. Let’s dive in.
Moving our robot Link to heading
So far, we haven’t made our robot actually do anything. It just sits there. To do this, we’re going to need a feature of Gazebo we haven’t used yet: plugins. Plugisn are pre-compiled pieces of code we can insert into our simulation from shared libraries. They can control all kinds of things, from the movement of models to features of the environment, like lighting.
Specifically, we’ll use a plugin called diff_drive. This plugin is designed for robots that can be “differentially driven”.
Think of a robot like the Roomba vacuum cleaner, which (much like our robot) has two motorized wheels near the edges of its body, and a single caster wheel at the front. That robot can turn in place by driving one wheel forward while the other one goes backwards. Tanks and other tracked vehicles also steer in this way. That’s “differential drive”.
We can add a plugin to our .sdf file with the <plugin> tag, like so:
<plugin
filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.2</wheel_separation>
<wheel_radius>0.4</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<topic>cmd_vel</topic>
</plugin>
You can place this code inside the vehicle_blue model, just above the closing </model> tag. Let’s break down the properties here:
filenameis the name of the library file the plugin comes fromnameis the name of the specific plugin we’d like to use, from said library<left_joint>and<right joint>define the joints attached to the wheels we’ll be controlling with differential drive<wheel_separation>and<wheel_radius>tell the differential drive plugin how far apart the wheels are and how large they are (important when figuring out how far to move the wheels when commanding the robot to turn or drive at a given rate of speed)<odom_publish_frequency>sets the frequency at which odometry data is published to/model/vehicle_blue/odometry<topic>tells the plugin which ROS topic the plugin should respond to. You may remember ROS from this earlier blog post. We’ll talk more later on about how to get ROS 2 Humble and Gazebo Fortress working together.
Topics and messages Link to heading
Time to get back to some ROS topics (pun intended). Now that we’ve added the DiffDrive plugin to our model, we should be able to drive it around our world.
We will do this by sending messages to the /cmd_vel topic, to which the DiffDrive topic is subscribed. Whenever we send a message to /cmd_vel, the DiffDrive plugin will process that message and move our robot in accordance with the content of the message.
We’ll start by doing this without involving ROS (we will cover integrating with ROS later on). For now, start by launching the Gazebo world with:
ign gazebo building_robot.sdf
Then, publish a message to the /cmd_vel topic directly using Gazebo’s ign command, like this:
ign topic -t "/cmd_vel" -m ignition.msgs.Twist -p "linear: {x: 0.5}, angular: {z: 0.05}"
Note: You will need to press the “play” button in the simulation, in order to see any movement.
Awesome! The robot moves (as you can see in the .gif below):
Sorry for the low quality…I am running my experiments on a remote desktop accessed via a web browser, so there’s bound to be some compression artifacts.
Controlling our robot with the keyboard Link to heading
Controlling the robot with ign topic -t would get very tedious, so let’s try controlling our robot with the keyboard instead. To do this, we will need two more plugins:
KeyPublisher, which will capture keystrokes and send them to the topic/keyboard/keypressTriggeredPublisher, which will take messages from an input topic, check them against a set of user-specified criteria, then produce user-defined messages on an output topic. We’ll use this to convertKeyPublishermessages into theTwistmessages expected by/cmd_vel.
Playing with KeyPublisher
Link to heading
If you want to see what KeyPublisher is doing, you can open two terminals.
In the first terminal, type:
ign gazebo buildng_robot.sdf
From the Gazebo GUI (upper right-hand corner) choose the little vertical ellipsis (three dots). Choose Key Publisher from the menu that opens up. Didn’t get that? No worries, the Gazebo GUI can be confusing first. Take a look at the screenshots below to get a better idea how to open up Key Publisher:
In another terminal, type:
ign topic -e -t /keyboard/keypress
Return to the gazebo GUI for a moment and press a few random keys on your keyboard. When you return to the terminal where you just ran ign topic -e -t /keyboard/keypress, you should see some output, similar to this:
data: 16777248
data: 16777235
data: 16777234
data: 16777237
data: 16777236
data: 16777235
data: 16777234
These correspond to the keys I pressed while I had the Gazebo GUI open.
What we want to do is map keypresses into Twist message which we can send to /cmd_vel.
To do that, we need to add another plugin to our .sdf file: the TriggeredPublisher. Here’s the code we need to add (we can add it just above the </world> tag. Do not add this plugin to your robot model. TriggeredPublisher should live outside the robot model: it’s not a property or function of the robot.
<!-- Moving Forward-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777235</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.0}
</output>
</plugin>
The code 16777235 in the code above corresponds to the “up” arrow key. Here’s the full set of codes for all arrow keys:
- Left ← : 16777234
- Up ↑ : 16777235
- Right → : 16777236
- Down ↓ : 16777237
Following along with the official tutorial in the Gazebo docs, let’s extend our TriggeredPublisher to cover all of the following motions, one for each key:
- Left ➞ 16777234 ➞ linear: {x: 0.0}, angular: {z: 0.5}
- Up ➞ 16777235 ➞ linear: {x: 0.5}, angular: {z: 0.0}
- Right ➞ 16777236 ➞ linear: {x: 0.0}, angular: {z: -0.5}
- Down ➞ 16777237 ➞ linear: {x: -0.5}, angular: {z: 0.0}
Expanding on our TriggeredPublisher code a bit, we get:
<!-- Up arrow: move forward-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777235</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Down arrow: move backward -->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777237</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: -0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Left arrow: turn left -->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777234</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.5}
</output>
</plugin>
<!-- Right arrow: turn right -->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777236</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: -0.5}
</output>
</plugin>
Make sure you add all this code just before the </world> tag, then safe your .sdf file.
If you start up your Gazebo world again with ign gazebo building_robot.sdf, load the Key Publisher plugin from the GUI (as described above) and press a couple of keys, you should see your robot moving around. As usual, don’t forget to press “Play” first.
Awesome! That’s it for moving the robot. In our next post we’ll look at doing more with our world.
The (completed) .sdf file
Link to heading
Here’s the complete .sdf file. If you’ve followed along, yours should be just like this one (give or take a couple of blank lines or comments):
<?xml version="1.0"?>
<sdf version="1.8">
<world name="car_world">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-user-commands-system.so"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='vehicle_blue' canonical_link='chassis'>
<pose relative_to='world'>0 0 0 0 0 0</pose>
<link name='chassis'>
<pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
<inertial> <!--inertial
properties of the link mass, inertia matix-->
<mass>1.14395</mass>
<inertia>
<ixx>0.095329</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.381317</iyy>
<iyz>0</iyz>
<izz>0.476646</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
<!--let's
add color to our link-->
<material>
<ambient>0.0 0.0 1.0 1</ambient>
<diffuse>0.0 0.0 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
</collision>
</link>
<link name='left_wheel'>
<pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.043333</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.043333</iyy>
<iyz>0</iyz>
<izz>0.08</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<!--The
same as left wheel but with different position-->
<link name='right_wheel'>
<pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles
are in radian-->
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.043333</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.043333</iyy>
<iyz>0</iyz>
<izz>0.08</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<frame name="caster_frame" attached_to='chassis'>
<pose>0.8 0 -0.2 0 0 0</pose>
</frame>
<!--caster
wheel-->
<link name='caster'>
<pose relative_to='caster_frame' />
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.016</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.016</iyy>
<iyz>0</iyz>
<izz>0.016</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 1 0.0 1</ambient>
<diffuse>0.0 1 0.0 1</diffuse>
<specular>0.0 1 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
<joint name='left_wheel_joint' type='revolute'>
<pose relative_to='left_wheel' />
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz> <!--can
be defined as any frame or even arbitrary frames-->
<limit>
<lower>-1.79769e+308</lower> <!--negative
infinity-->
<upper>1.79769e+308</upper> <!--positive
infinity-->
</limit>
</axis>
</joint>
<joint name='right_wheel_joint' type='revolute'>
<pose relative_to='right_wheel' />
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz>
<limit>
<lower>-1.79769e+308</lower> <!--negative
infinity-->
<upper>1.79769e+308</upper> <!--positive
infinity-->
</limit>
</axis>
</joint>
<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>
<plugin
filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.2</wheel_separation>
<wheel_radius>0.4</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<topic>cmd_vel</topic>
</plugin>
</model>
<!-- Up arrow: move forward-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777235</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Down arrow: move backward -->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777237</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: -0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Left arrow: turn left -->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777234</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.5}
</output>
</plugin>
<!-- Right arrow: turn right -->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777236</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: -0.5}
</output>
</plugin>
</world>
</sdf>