ROS2 and Gazebo: Localization and Path Planning

Project Setup


Until now, I haven't paid a lot of attention to the way maps were configured, as it wasn't of any importance. Now things changed, so let's stress out the following:
We have two robots in our project, and each of them is an independent one. It means that, if Internet is down, robots should still function and at least be able to return home.

That means they have to keep their own maps. Now, that is already implemented: instead of creating a centralized map server, I have separate map servers, one per robot. However, it also means that RViz receives two maps. You have probably noticed it, when in a SLAM session, one map is less bright: this is because maps are semi-transparent, and whichever map is displayed on top, is brighter:

Same applies to keepout maps: there are two of them.

For the RViz to display these maps in proper places, relative to each other, we need to make sure the "fixed frame" in RViz is set to "world", not to map or odom.

RViz config files are available in the archive.


The maze_with_charger.sdf world changed, too. It now has spherical_coordinates set to a random place in USA. What is important, same coordinates are used in code to initialize GPS: now ROS2 knows how to link map's X,Y to GPS coordinates.


As I am adding new files to the scripts folder (multi_bot_nav_01/multi_bot_nav_01) folder, I need to make them available in a package. To do it, I have to modify the


Configuration files

In a config folder, we have two config files: nav2_params.yaml and nav2_params_multi.yaml, one is for a single robot project, another is for multi robot project.

The difference is in the way I treat frames and topics: for multi robots case they have prefix (robot's name) so each robot can get its own messages. I believe it is possible to make it a single file, but it looks like keeping them separate is easier. For details, see earlier sections, where I created a multi robot project.

These config files are very important for localization, as they hold parameters for AMCL and Kalman Filter. Important thing here is that usually, in tutorials, you will see people creating two robot localization nodes, one with GPS and one without it.

I do not use this approach: GPS is not accurate enough, so it can only be used for rough position estimation; I use raw GPS for that purposes, plus filtered position produced by ROS2 robot localization node from /odom, /imu and /scal (lidar).

In the project, a PathFollower class is provided; as it moves the robot along the path, it gathers location data, so we can create a chart. You can see that amcl provides coordinates that strongly corellate with "odom" and GPS is... well, we can use it for initial pose estimation.

ROS2 uses amcl to run particle filters (see links section), robot localization node uses Kalman Filter, and together they provide a rather accurate pose estimation. However, keep in mind that in ROS2/Nav2 it all is true only for "flat" 2D map where 2D lidar shines.

Note that there are many non-obvious things that you will discover when working with settings files. Here is one example: suddenly, robot localization node doesn't publish filtered odometry with a strange message:


There is a lot of discussions on forums, main "cure" being to synchronize time between computers (I only have one), or to rebuild ROS2 from sources...

What I found was, I used "base_footprint", while I was supposed to use "base_link", a very non-obvious reason... However, I was still not able to figure out how to make robot_localization_node to work with namespaces. What that means: In the code (, I have a class member self.current_pose. Now, in a single robot project, it is possible to set this variable in the callback for filtered odometry. Filtered odometry is published by robot_localization_node (ekf) and it contains the most accurate combination of odom, imu and lidar data.

And it doesn't work with namespaces, so in multi-robot project I have to set the self.current_pose in AMCL callback. Not a big deal, but...

Launch files

The only launch file that changed is "main", the rest of files are the same, and they work the same way, allowing access to localization and Nav2 functionality. Below, in a "Compiling and Running" section, I provided the corresponding commands.

As for multi_simulation_launch, let's take a look at important aspects of the code:


As you can see, the only important difference is that we used the robot localization node.

(C), all rights reserved

Please read the disclaimer