ros2_control extra bits

There are a few follow-up points regarding ros2_control for our differential drive robot that are worth sorting out before we move ahead with the rest of the tutorial series. I've kept these out of the main video to avoid further detracting from the core explanation of how to get ros2_control up and running.

Update URDF to match robot structure

The first step is to update your URDF to match your physical robot (if you have one). In my case I did the following:

• Set up xacro parameters for all the key dimensions and set them accordingly
• Changed my Gazebo and RViz colours to match the real robot
• Fixed up the controller YAML to have the correct wheel separation and radius
• Added an extra xacro file for a smiley face on the front, like my real one currently has (it will eventually have a screen)

You can check out the differences here.

Problem 1 - Gazebo clock rate

At the end of the previous tutorial I mentioned that there are a few problems with the simulation. The first of these is that you'll notice the robot motion in RViz is quite "choppy" while driving the robot around. That’s because the Gazebo-ROS clock is only running at 10Hz, so the rest of ROS gets locked to that slow speed. We need to tell Gazebo to publish the clock faster. This isn’t only a visual thing, it’s actually causing our odometry to drift quite badly (but don’t check that now because there are other problems affecting it too which we’ll fix soon).

Fixing the clock rate is a bit annoying. First we need to create another parameter file, a good name is gazebo_params.yaml. In it we’ll put the following code, which is telling gazebo to publish the clock at 400Hz instead of 10Hz (if your computer is slow you may need to reduce this).

gazebo:
  ros__parameters:
    publish_rate: 400.0

We now need Gazebo to see these parameters when it starts up. In our launch_sim.launch.py we are "including" the ROS-provided Gazebo launch. This launch file has an argument called extra_gazebo_args which lets us just send whatever we want down to the Gazebo it runs. In this case, we'll create a variable to store the path of our params file (similar to how we handled the URDF), and then combine that path into a string telling Gazebo to read it.

gazebo_params_path = os.path.join(
                  get_package_share_directory(package_name),'config','gazebo_params.yaml')

gazebo = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
                launch_arguments={'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_path }.items()
         )

Note that future versions should let us pass the params file in directly as a launch argument, simplifying this process ever so slightly.

If we rerun Gazebo now we’ll see that RViz is much smoother.

On my computer I also get an error printed in the Gazebo terminal which is a bit annoying, I think the Gazebo command line parser is treating the file as an extra SDF to load and failing. We can just ignore it, but if you’ve got a fix then let me know!

Problem 2 - Wheel drift

The next problem we have is wheel drift. The easiest way to spot this is to do a fresh start on Gazebo/RViz, and do a full revolution of the robot. We should see that when we come back to the initial angle in Gazebo, RViz is no longer in sync.

There could be a few factors affecting this, but an easy one to tackle is that the wheel collision is a cylinder, which means there will be subtle skidding effects occuring due to the ground connection being a line. We could somewhat alleviate this by reducing the thickness of the wheel collision cylinder, but better is to reduce it infinitely, by replacing the cylinder with a sphere. Now the contact with the ground will be a single point per wheel.

All we need to do is in our left/right wheel links of our URDF, in the collision geometry (not visual!) replace cylinder with sphere and remove the length parameter. To check this we can enable collision view in Gazebo to see the collision sphere and the visual cylinder.

There may be better methods than this - if you have one please let me know!

Additionally, if you are still having issues with wheels skidding and drifting oddly, try experimenting with the friction parameters like we did for the caster.

If we relaunch it we should see that a full rotation places us back where we started.

For what it’s worth, the reason this wasn’t a problem with the old plugin is that it actually cheats to provide the odom transform perfectly rather than calculating it based on the simulated wheel spin.

Problem 3 - Sensor synchronisation issue

Now it’s time for problem number 3, which I currently don’t have a fix for, and if someone can figure out what’s going on, I’ll be very grateful. I have a whole GitHub repo and separate video for reproducing it.

It's difficult to describe in text (just watch the video) but essentially, when using ros2_control with Gazebo the sensor output appears to be out of sync with the published transforms.

I don't think it's a friction issue, as I have tried to eliminate that. It seems more like a timing issue, but the odd thing is that the laser data moves in the direction of rotation, not away from it, suggesting that the transforms are being published earlier than the sensor data rather than later (so it's not just be a delay in ros2_control).

It could be to do with acceleration limits, I have tried to experiment with this too with no success. Keeping accelerations and velocities low reduces the effect, which may be useful in testing.

Swapping between control methods

Given that we have these issues, we might sometimes want to swap back to the old control method. It generally tends to work a bit more "perfectly" which is not always what we want but can be handy.

Taking the steps below will let us easily swap between the Gazebo diff drive plugin and the ros2_control one.

Add xacro argument & condition

Using xacro to build our URDF gives us the ability to pass in an argument at runtime to change the result. This could be anything like the length of an arm, the colour of a wheel, but in this case we want to pass in a boolean to tell it whether to use ros2_control or not.

We’ll start by adding an argument declaration at the start of our robot.urdf.xacro, and default it to true (we generally do want to use ros2_control).

<xacro:arg name="use_ros2_control" default="true"/>

Now we need to add a conditional to change which bit of the URDF is used. Xacro doesn’t have an if-else construct that we may be familiar with, instead it has if and unless which will trigger depending on whether the condition is true, or untrue. So down where we have our gazebo_control.xacro commented out, we instead want to add the following blocks:

<xacro:if value="$(arg use_ros2_control)">
    <xacro:include filename="ros2_control.xacro" />
</xacro:if>
<xacro:unless value="$(arg use_ros2_control)">
    <xacro:include filename="gazebo_control.xacro" />
</xacro:unless>

So, if use_ros2_control is true, include the ros2_control.xacro file, and unless use_ros2_control is true, include gazebo_control.xacro.

Pass in the xacro argument

Now we need to pass the xacro argument in when we process the file, which is in our robot state publisher launch file (rsp.launch.py).

This should be straightforward but actually isn’t. Right now this file is using the xacro Python module to process the file, which is all well and good, and it has the option (don’t type this!) to set arguments by specifying the mappings like so:

xacro.process_file(xacro_file, mappings={'use_ros2_control': 'true'})

This is fine, and works as is, but ideally we want to be able to set it at the level above this, to pass it in as a launch argument and because of how launch files are processed, dropping a LaunchConfiguration variable in here like we did with use_sim_time just won’t work.

The workaround is a little hacky but it’s not too bad, especially considering launch scripts aren’t designed to be super python-heavy anyway. Instead of using the xacro Python module we’re going to call the xacro process using the launch Command substitution.

We’ll go ahead and make the launch argument anyway, right under our use_sim_time one.

use_sim_time = LaunchConfiguration('use_sim_time')
use_ros2_control = LaunchConfiguration('use_ros2_control')

Next, to make things smoother in case we ever want to go back to the old one, we'll move the toxml() up to the process call. Hopefully you can see that that won’t really change anything except that robot_description_config will now be the final string (rather than some xacro object), which makes more sense anyway.

## Was
robot_description_config = xacro.process_file(xacro_file)
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}

## Now
robot_description_config = xacro.process_file(xacro_file).toxml()
params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time}

So the params is now taking in a string. But because it is a part of the clever ROS Launch system, instead of a string it can also take a substitution. So we'll take advantage of the Command substitution, and replace the robot_description_config assignment to a call out to the xacro process rather than the Python module. Make sure you also add the appropriate import at the top.

# At the top
from launch.substitutions import Command

# Down where we were

#robot_description_config = xacro.process_file(xacro_file).toxml()
robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control])

This line is just constructing a command that could be typed into a terminal, but instead it’s going to run it inside the launch script and use the output. We can see that use_ros2_control (a LaunchConfiguration argument) is used in this call, so at runtime it will substitute that for whatever is passed in, then substitute the result of the whole command into the robot_description_config variable as a string.

Pass in the Launch argument

Then in launch_sim.launch.py where we were including our robot state publisher launch, we can set it to whatever we want by updating the launch_arguments line when including rsp.launch.py:

...
launch_arguments={'use_sim_time': 'true', 'use_ros2_control': 'true'}.items()
...

This is what I demonstrated in the video, however it would be nice to be able to toggle it at runtime, i.e. by running:

ros2 launch my_package launch_sim.launch.py use_ros2_control:=false

There are actually two ways to achieve this. The first is to...do nothing. If we undo the previous step where we passed down the launch argument, we'll find that we are able to set it at the command line and it will automatically find its way down to where it needs to be.

Alternatively, we can create another launch argument, this time in launch_sim.launch.py (probably with the same name), and pass it down like

use_ros2_control = LaunchConfiguration('use_ros2_control')

...

launch_arguments={'use_sim_time': 'true', 'use_ros2_control': use_ros2_control}.items()
...

These will have the same effect, but the latter is a bit more explicit. We should also remember to add an appropriate DeclareLaunchArgument entry.

This has probably all been a bit confusing, so check out the final files below.

rsp.launch.py:

launch_sim.launch.py:

RViz Config

One last thing I did was to set up RViz the way I liked it and saved the configuration to the config directory so that it can be loaded back up again in future with rviz2 -d.

Conclusion

Now that we've got those things tidied up a bit, we can move forward with ros2_control on our robot, and then start to run some algorithms!