libgazebo_ros_diff_drive.so: 2 wheels
libgazebo_ros_diff_drive.so vs libgazebo_ros2_control.so
To make our life even more entertaining (why else?) Gazebo has its own differential drive implemented, and ROS2 has a different one as part of "controllers".
The difference is, Gazebo's one is easier to use, while ROS2's one works with any hardware that has support for it, not just with Gazebo.
Directory structure
In our ROS2 workspace ("harsh"), we need to create the following directory tree:
$ tree --dirsfirst src/diff_drive_bot
src/diff_drive_bot
├── config
├── description
│ ├── camera.xacro
│ ├── colors.xacro
│ ├── gazebo_control.xacro
│ ├── inertial_macros.xacro
│ ├── lidar.xacro
│ ├── robot_core.xacro
│ └── robot.urdf.xacro
├── launch
│ ├── launch_sim.launch
│ └── rsp.launch.py
├── meshes
│ └── body.stl
├── rviz
│ └── urdf_config.rviz
├── CMakeLists.txt
└── package.xml
Note that files, both xacro macros files and some other (usually, they have xacro extension, too) are included in each other, in other words, we can (technically) merge camera, lidar, colors etc. into one file. So most of the files are just parts of one (URDF) file, stored separately for ease of maintenance.
empty.yaml is an empty file, we don't even need it:
but we need a configfolder.
camera.xacro contains 3d design and properties of a camera.
colors.xacro is used to define colors
gazebo_control.xacro is used to specify which wheels we need
to attach to our controller.
inertial_macros.xacro is used to make our life a bit easier:
having an object, this macro calculates values and fills the
corresponding properties for inertia. For example, if we have a cube,
we may want to place center of inertia in its center, and assign mass,
based on cube's volume.
Note that for real life objects center of mass is not always
located in geom. center of an object and mass can not always
be calculated based on volume; therefore, this is a simplified
approach for our simple model only.
lidar.xacro contains 3d design and properties of a lidar.
robot.urdf.xacro is the place where we include all files.
robot_core.xacro is a 3d design of our robot.
launch_sim.launch is a file we use to launch both our bot
and the world, both in Gazebo and in RViz.
body.stl is a mesh, which is a 3d shape of a more
advanced (than cubes and cylinders) shape.
rsp.launch.py is a file to launch our robot (it will be
called from launch_sim.launch, so that both world and
robot are launched).
urdf_config.rviz: to start, RViz requires this file.
It is just something I copied, no changes made.
empty.world: this world contains a plane for ground,
so that our model doesn't fall, plus source of light.
shapes.sdf is a "world" that I have copied from the Internet.
We need few objects in the world if we want to test lidar and camera.
CMakeLists.txt: used to build the project.
package.xml: dependencies and description.
The code for this section is located in diff_drive_bot part of an archive. Note that archive includes some additional folders, like maps and worlds, that are used by all projects and therefore are located outside of them.
robot.urdf.xacro
ROS2 tutorials that are available in the Internet belong to two categories:
either they provide you with link to GitHub with no explanations, or they
explain whatever the author can remember in few minutes he/she
allocated for writing a tutorial, but provide no complete code.
Let's fix it.
The code was provided above, and here we'll go over the files, so
you can get some ideas on what it is about. I have allocated at least
10 minutes to this tutorial, so it should be at least twice
as detailed as the average ones in the Internet :)
robot.urdf.xacro is a top-level URDF file, the place where
other parts of URDF are included. Just to make it clear: robot and
world are "described" in URDF files. We could have provided
a single large URDF for, say, a robot, but then again, wheels
are almost identical, colors are just few, so we used xacro files
for those, and included them in robot's URDF, instead of pasting it.
Also, lidar/camera files, if pasted into main robot file, would make
it harder to read, so they were moved to separate files as well.
Using the xacro utility as described above, you can produce a
single large URDF file, just to see what it looks like when all macros are
"expanded".
Here is what robot.urdf.xacro looks like:
The order of inclusion maters, for example, we want camera.xacro to be included before it is used in robot_core.xacro
Note that xacro files have robot tags, note also, that only main file has name parameter in it.
Let's check if it is valid:
$ check_urdf src/fdiff_drive_bot/urdf/robot.urdf.xacro
colors.xacro
As you can see, we have simply specified colors. For example, blue is 02 (red), 0.2 (green) and 1 (blue): RGB color model is used. I suspect that the 4th parameter is transparency, but I never tried to play with it.
Later you will see how these colors can be used.
inertial_macros.xacro
Copied some time ago somewhere in the Internet (I am pretty sure the site is among ones in "sources" section below). For simple models it makes your life easier, as you don't have to calculate parameters for center of inertia and mass.
camera.xacro
This is a rather standard camera with minimum customizations.
Camera is controlled by ROS2 plugin libgazebo_ros_camera.so, and this xacro file provides parameters for it. It also provides physical dimensions of a camera, and a "joint" that attaches camera to the rest of a robot.
Lidar
ROS2 supports lidars via libgazebo_ros_ray_sensor.so plugin. As in the case with camera, this xacro file provides data for this plugin to use (to customize the lidar), as well as physical dimensions and "joint" to attach the lidar to a robot.
Lidar is based on so-called ray sensor. We can specify the distance, frequency and density of a scan, angle of view and so on.
Note that there are 1d, 2d (ours included) and 3d lidars.
robot_core.xacro
This is where we keep the description of our robot.
Let's view it, line by line.
First, we need a header, it is pretty standard:
Then we include our colors. Note that we could have included them in the robot.urdf.xacro instead, but then we wouldn't have colors if we run our robot description file (this one) alone:
Traditionally, the "origin" of a robot is called base_link. Link in URDF is not a link at all, but an object, while what we would normally call a link (something connecting two objects) is called a joint:
As you can see, this link has no dimensions, colors or whatever else. It is just a place where everything else will be attached.
The "body" of our robot is just a box, traditionally, though not always, it is called chasis:
It is a box, with dimensions specified, it also has white color (notice
how we used our colors macro).
Additionally (to the visual property above), it has a collision
property. It is convenient, as to calculate collisions we need a lot of computer
resources, so to make computations faster, we can use a simple shape for it.
Also, collision box can be a big larger than the physical box - just in case.
Finally, we use our xacro:inertial_box maxro from another include file,
so we don't have to calculate the center of the box.
This was a simple shape, and our robot now looks like a box:
Let's make it more vacuum-cleaner shaped. To do it, we need to use meshes. Gazebo can use different 3d files as meshes, including STL files. So to create a "cylinder cut on both sides" shape, I have logged in the wonderful online 3d editor called Tinkercad, and created the STL file in less than a minute. I have saved it as a body.stl in meshes folder.
Now we can modify our chasis link, using mesh instead of a box shape:
Colors that we used in URDF are not compatible with Gazebo - for historical reasons. For RViz, but not for Gazebo. So to paint our robot in Gazebo, we need to include Gazebo-specific tags:
Now we need to attach the chasis box to the base_link. We are going to use a fixed (unmovable) joint:
We are going to create wheels, using approximately the same approach. A wheel is just a cylinder, and it is going to be attached to a base_link using a "continuous" joint, whish is type of joint that can rotate at any angle, just like a wheel should:
In a real world, a caster wheel usually has different design :) but we are just going to submerge a sphere in the body of our robot and reduce its friction. Once again: it will not spin. Instead, it will slide (think of teflon semi-sphere):
gazebo_control.xacro
The diff. drive control, implemented in libgazebo_ros_diff_drive.so uses this file to get information about robot's parameters, like the distance between wheels etc. The file also specifies the output, things that have to be published (like wheel transformations).
As always when you deal with ROS (and especially ROS2) and Gazebo, there are many poorly documented parameters that you have to research yourself.
rsp.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import xacro
package_name = 'diff_drive_bot'
# world_file = 'room.world'
def generate_launch_description():
# Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')
# Process the URDF file
pkg_path = os.path.join(get_package_share_directory(package_name))
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
rviz_config_path = os.path.join(pkg_path,'rviz','urdf_config.rviz')
robot_description_config = xacro.process_file(xacro_file)
# Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
joint_state_publisher_node = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher'
)
joint_state_publisher_gui_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui'
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
# Launch!
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use sim time if true'),
DeclareLaunchArgument(name='rvizconfig', default_value=rviz_config_path,
description='Absolute path to rviz config file'),
node_robot_state_publisher,
joint_state_publisher_node,
joint_state_publisher_gui_node,
rviz_node
])
First, we include dependencies:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
import xacro
Then we build strings containing paths to files we need:
def generate_launch_description():
# Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')
# Process the URDF file
pkg_path = os.path.join(get_package_share_directory('diff_drive_bot'))
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
rviz_config_path = os.path.join(pkg_path,'rviz','urdf_config.rviz')
robot_description_config = xacro.process_file(xacro_file)
Then we describe nodes that we want to start:
# Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
Note that this is an URDF model, good for RViz, and generally, not for Gazebo. So we only create RViz node and a publisher.
Finally, we start nodes:
# Launch!
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use sim time if true'),
DeclareLaunchArgument(name='rvizconfig', default_value=rviz_config_path,
description='Absolute path to rviz config file'),
node_robot_state_publisher,
rviz_node
])
launch_sim.launch
This is our main launch file, both for robot and a world:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name='diff_drive_bot' # --- CHANGE ME
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'True'}.items()
)
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
)
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'diff_drive_bot'],
output='screen')
# Launch them all!
return LaunchDescription([
rsp,
gazebo,
spawn_entity,
])
First, we import dependencies:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
Then we include our launch file for a robot and for Gazebo:
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name='diff_drive_bot' #--- CHANGE ME
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'True'}.items()
)
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
)
To spawn a robot, we create a ROS2 node:
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'my_bot'],
output='screen')
Finally, we start it all:
# Launch them all!
return LaunchDescription([
rsp,
gazebo,
spawn_entity,
])
Starting the simulation
# Terminal 1:
# Do it once:
$ source /opt/ros/foxy/setup.bash
# Every time:
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard
# Terminal 2:
# Do it once:
$ source /opt/ros/foxy/setup.bash
# Every time:
$ cd ~/SnowCron/ros_projects/harsh/
$ colcon build --packages-select diff_drive_bot
$ source install/setup.bash
$ ros2 launch diff_drive_bot launch_sim.launch world:=src/worlds/shapes.sdf
The first terminal, when it has keyboard focus, will allow us to send commands to the bot. The second terminal starts the simulator:
Note: if you accidentally pressed Ctrl-Z instead of Ctrl-C when trying to exit Gazebo, Gazebo will be detached and asleep. To be able to close it:
$ jobs -l
[1]+ 14096 Stopped ros2 launch fwd_bot launch_sim.launch world:=src/worlds/shapes.sdf
# Now we can use this "1":
$ fg %1
# Then Gazebo is awake and can be killed:
$ killall -9 gzserver
$ killall -9 gzclient