Create SLAM example

[lukicdarkoo]

SLAM will be a great demo and proof that ROS2 is well integrated with e-puck2 (LaserScan, Odometry, transformations...). Therefore, it would be nice to create a launch file with configured SLAM node and RViz2 visualisation.

I am considering slam_toolbox (https://github.com/SteveMacenski/slam_toolbox/) as it supposed to be a default ROS2 solution for SLAM.

[DavidMansolino]

slam_toolbox seems indeed to be a good option!

[lukicdarkoo]

Here I will track my progress on this since there is no source code to be published before everything is configured.

Here is my overall plan

• Try if gmapping gives reasonable results
  • Configure ros2bridge and gmapping
  • Use configuration given here and run
  • Measure performances in simulation
• If successful, identify key parameters and reconfigure slam_toolbox
• Create an issue to slam_toolbox
  • Create GIFs of gmapping and slam_toolbox

Results

1. gmapping initial results

gmapping from ROS1 gives the following result:

Scan Matching Failed, using odometry. Likelihood=0
lp:0.147856 0.0681493 1.06407
op:0 0 0
Average Scan Matching Score=0
neff= 29.7575
Registering Scans:Done

even with the often low average matching score, the result is better than with slam_toolbox:

(black wall comes from two boxes positioned around the robot)

2. webots_ros2_epuck upgraded to use simulated time

There is a problem with simulation time. gmapping doesn't get static transforms if it is run after the simulation, otherwise, there is TF_OLD_DATA warning because of ROS clock. Static transforms should be latch topic, but since the transforms are not defined in the launch file it is kinda tricky to get desired behaviour.

3. ROS1 wasn't aware of ROS2 clock, solved

ROS1 wasn't aware that ROS2 uses simulation time, fixed by adding use_sim_time to ROS1 launch file:

<rosparam param="use_sim_time">true</rosparam>

4. Performance

After, a few tests gmapping works way better than slam_toolbox. I can assume that map we are getting from gmapping now would be just enough for all future demos. Next step is to achieve similar results on slam_toolbox by identifying key parameters.

This significantly shrinks the source of errors. This means that error is not related to the publishing of transforms (especially static transforms), odometry calculations that are not visualised in RViz (e.g. velocity), laser scan configuration, invalid header.frame_id for related topics and others. The mistake is most probably hidden in the configuration of slam_toolbox.

More representative result. It gives OK result (for e-puck capabilities) even though my odometry slipped when I hit the box.

5. Tunning slam_toolbox parameters

I created a new branch (https://github.com/cyberbotics/webots_ros2/tree/feature-slam) that allowed me to quickly evaluate different configurations and here are the results:

Issues are the following (key performance differences compared to gmapping):

• There are frequent jumps in position. It seems that slam_toolbox doesn't "trust" to odometry, but I couldn't find a parameter to change it.
• If a range is greater than the maximum range it is not considered. Our sensors have a very short range and very often we can "see" there is no object in the radius of 5cm. This could be valuable in map building. gmapping does it very well.
• Map updates are slow. It can be changed to send a new map more frequently but there are no new features in the map (it is not as fast as gmapping).

6. Issue posted

Issue posted to slam_toolbox:
https://github.com/SteveMacenski/slam_toolbox/issues/192