ROS2, diff. drive robot, ros2_control, 2 wheels 2 wheels


In this section we are going to use a more advanced approach: ROS2 controller. As I mentioned above, connecting ROS2 to Gazebo is not enough: we need to be able, at some point in future, to replace Gazebo simulation with a real world robot. Also, it would be great if this replacement requires changing of just few lines of code.

Now, Gazebo plugin ( only works with Gazebo. Of course, some hardware manufacturer might create a plugin for a particular hardware, that can be used instead of Gazebo... but they do not do it. The reason is, there are other simulators, like WeBots, Unity, Nvidia's one...

So a universal approach is used instead: a tool based on a control theory, that can be (provided a simulator supports it) plugged in any simulator. This is what ROS2 control is used for.

Directory structure

                    $ tree src/diff_drive_controller_bot/
                    ├── CMakeLists.txt
                    ├── config
                    │   │   └── my_controllers.yaml
                    ├── description
                    │   ├── camera.xacro
                    │   ├── colors.xacro
                    │   ├── gazebo_control.xacro    <--- REMOVED
                    │   ├── inertial_macros.xacro
                    │   ├── lidar.xacro
                    │   ├── robot_core.xacro
                    │   ├── robot.urdf.xacro
                    │   └── ros2_control.xacro      <--- ADDED
                    ├── launch
                    │   ├── launch_sim.launch
                    │   └──
                    ├── meshes
                    │   └── body.stl
                    ├── package.xml
                    └── rviz
                        └── urdf_config.rviz


The source code is available here.
The base of the code is nearly identical to what we had for a 2-wheeled robot with Gazebo plug-in. However, we have removed support for Gazebo plug-in itself (gazebo_control.xacro) and replaced it with support for ros2_control (ros2_control.xacro).



This file has two sections: ros2_control and gazebo. The ros2_control section declares the hardware we are going to use: if we switch from simulation to a real robot, this is the place we are going to change.
Note that to make such a change, we need to either have the hardware driver (provided by manufacturer) or write it ourselves.
Right now, we use Gazebo as a "hardware".

We also list joints (same as we have in robot's URDF file) that the diff. drive controller is going to control. Currently, ROS2 supports velocity, position and effort command interfaces: I used velocity.

A gazebo section of a file specifies the control library we use and tells it where to look for configuration file (my_controllers.yaml).


This file contains settings that the diff. drive controller will use.

                    update_rate: 30
                    use_sim_time: True
                        type: diff_drive_controller/DiffDriveController
                        type: joint_state_broadcaster/JointStateBroadcaster
                        publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
                        base_frame_id: base_link
                        left_wheel_names: ['left_wheel_joint']
                        right_wheel_names: ['right_wheel_joint']
                        wheel_separation: 0.35
                        wheel_radius: 0.05
                        use_stamped_vel: false
                        # --- The rest can be removed ---

                        wheel_separation_multiplier: 1.0
                        left_wheel_radius_multiplier: 1.0
                        right_wheel_radius_multiplier: 1.0
                        publish_rate: 50.0
                        odom_frame_id: odom
                        base_frame_id: base_link
                        pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
                        twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
                        open_loop: true
                        enable_odom_tf: true
                        cmd_vel_timeout: 0.5
                        #publish_limited_velocity: true
                        #velocity_rolling_window_size: 10
                        # Velocity and acceleration limits
                        # Whenever a min_* is unspecified, default to -max_*
                        linear.x.has_velocity_limits: true
                        linear.x.has_acceleration_limits: true
                        linear.x.has_jerk_limits: false
                        linear.x.max_velocity: 1.0
                        linear.x.min_velocity: -1.0
                        linear.x.max_acceleration: 1.0
                        linear.x.max_jerk: 0.0
                        linear.x.min_jerk: 0.0
                        angular.z.has_velocity_limits: true
                        angular.z.has_acceleration_limits: true
                        angular.z.has_jerk_limits: false
                        angular.z.max_velocity: 1.0
                        angular.z.min_velocity: -1.0
                        angular.z.max_acceleration: 1.0
                        angular.z.min_acceleration: -1.0
                        angular.z.max_jerk: 0.0
                        angular.z.min_jerk: 0.0
                    ## joint_broad:
                    ##   ros__parameters:

In this file we provide parameters for diff. drive controller (diff_cont), like distance between wheels, and specify joint state broadcaster (joint_broad). Note that we are going to use these two names in our launch file.

Note the cmd_vel_timeout: 0.5 line: this is how long the command works. So if you don't want your robot to stop after 0.5 seconds, you can increate value of this parameter (not an elegant solution, we better send a new command ourselves).



This is the "main" robot description file that includes all other files. Note that, compared to the previous example, we have switched from gazebo_control.xacro to robot_core.xacro.

I have to admit that my current implementation is a bit messy. The original idea was to have robot launch file in and robot + Gazebo world launch file in launch_sim.launch that calls it plus launches the world.
As we want to be able to switch from simulation to real robot and back, we probably need parameter-based condition in our launch files, something like

                    if is_simulation != 'true':
                        # Use Gazebo as a hardware
                        # Use real hardware

This is not implemented yet, and I don't thing it is a good idea to add this sort of changes to a simple tutorial.

Currently, the starts node_controller and node_robot_state_publisher, but to run the simulation, we also need to create the (mentioned above) joint_broad and diff_cont. Also, the order maters, so I am doing it in a launch_sim.launch.

                    import os

                    from ament_index_python.packages import get_package_share_directory
                    from ament_index_python.packages import get_package_share_directory
                    from launch import LaunchDescription
                    from launch.actions import DeclareLaunchArgument
                    from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
                    from launch.event_handlers import OnProcessExit
                    from launch.conditions import IfCondition
                    from launch.launch_description_sources import PythonLaunchDescriptionSource
                    from launch.substitutions import LaunchConfiguration
                    from launch_ros.actions import Node
                    import xacro
                    package_name = 'diff_drive_controller_bot'
                    def generate_launch_description():
                        use_sim_time = LaunchConfiguration('use_sim_time')
                        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)
                        controller_config = os.path.join(pkg_path, "config", "my_controllers.yaml")
                        robot_description = {"robot_description": robot_description_config.toxml()}
                        node_controller = Node(
                            parameters=[robot_description, controller_config],
                        params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
                        node_robot_state_publisher = Node(
                        node_rviz = Node(
                            arguments=['-d', LaunchConfiguration('rvizconfig')],
                        # Launch!
                        return LaunchDescription([
                                description='Use sim time if true'),
                            DeclareLaunchArgument(name='rvizconfig', default_value=rviz_config_path,
                                description='Absolute path to rviz config file'),            


Here we run both our and also launch controllers and Gazebo with world file:

                    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.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
                    from launch.event_handlers import OnProcessExit
                    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_controller_bot' #--- CHANGE ME
                        rsp = IncludeLaunchDescription(
                                    )]), launch_arguments={'use_sim_time': 'true'}.items()
                        # Include the Gazebo launch file, provided by the gazebo_ros package
                        gazebo = IncludeLaunchDescription(
                                        get_package_share_directory('gazebo_ros'), 'launch', '')]),
                        load_joint_state_controller = ExecuteProcess(
                            cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
                        load_diff_drive_base_controller = ExecuteProcess(
                            cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
                        # Run the spawner node from the gazebo_ros package. The entity name not really matter if you only have a single robot.
                        spawn_entity = Node(package='gazebo_ros', executable='',
                                            arguments=['-topic', 'robot_description',
                                                    '-entity', 'diff_drive_controller_bot'],
                        # Launch them all!
                        return LaunchDescription([

Starting the simulation

The only difference here is that ROS2 diff drive controller "listens" to a different topic, so if we want to use teleop_twist_keyboard, we have to remap its topic:

                    # Terminal 1:
                    $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped
                    # Terminal 2:
                    $ cd ~/SnowCron/ros_projects/harsh/
                    $ colcon build --packages-select fwd_bot
                    $ source install/setup.bash
                    $ ros2 launch fwd_bot launch_sim.launch world:=src/worlds/shapes.sdf 

(C), all rights reserved

Please read the disclaimer