GMapping and RPLidar

Previous post ends with a question: why Hector Slam perform better than GMapping when in theory should be the opposite? The right question should be: why GMapping works bad with RPLidar? Since I'm not the only developer asking this question I hope will be helpful for everyone share my results.

There are several reasons for which RPLidar and GMapping does not work well together:

• the LaserScan message expected by GMapping;
• the RPLidar reference frame;
• the LaserScan message created by RPLidar node.

The good new is that all issues can be easily fixed. On this post I will show you how I success to make RPLidar and GMapping work together.

The LaserScan expected by GMapping

First issue regards the LaserScan expected by GMapping. Unfortunately this is not documented anywhere and I found it only checking out source code: on latest commits (not yet released) a new check was added to make sure LaserScan message min angle and max angle are opposite:

maxAngle = - minAngle

This condition is not satisfied by RPLidar node but, since released version of GMapping does not include this check, is not easy to notice.

The RPLidar reference frame

Second issue came from RPLidar frame and RPLidar rotation direction. It assume positive angle in CW direction: in order to reflect this orientation Z-axis must be directed to the RPLidar's bottom side (this is not a bug but, since this is not pointed out in documentation, is quite common to make a mistake broadcasting laser scan frame).

Further more, in order to satisfy the GMapping required condition about angles shown on previous section, is better to use a frame rotated by 180° respect one shown in RPLidar documentation: using this frame the resulting min  and max angle will be (approximately) equals to -3.14 and 3.14 according to GMapping condition.

The RPLidar frame from RoboPeak documentation

The RPLidar frame must be used in ROS

The LaserScan message created by RPLidar node

The RPLidar node require some modification in order to reflect corrections shown before about min and max angle (they must take count of the frame rotated by 180°):

scan_msg.angle_min =  M_PI - angle_min;

scan_msg.angle_max =  M_PI - angle_max;

Applying this fix will result in negative value of published angle_increment according to the fact that RPLidar rotate CW.

GMapping configuration

GMapping default settings are designed for high speed and large range lidars. RPLidar has really good value/price rate but it does not fit those requirements so modifications of GMapping parameters  is needed in order to have best result.

Those are the GMapping parameters I used with best results:

maxUrange: 5.5

maxRange: 5.5 it was 6.0 but after better tuning 5.5 is suggested

minimumScore: 50

linearUpdate: 0.2

angularUpdate: 0.25

temporalUpdate: 5.0

delta: 0.025

Conclusion

In conclusion RPLidar can works with GMapping with good performance after those issues are fixed:

The GMapping built map before fixes

The GMapping built map after fixes. Unfortunately was not possible to use the same bag file for this test since the LaserScan message was changed. Anyway the test was conducted on same environment with same conditions.

The RPLidar node with fixes it is available on GitHub repo.

Navigation stack test: GMapping vs Hector Slam

After first failed test of Geduino navigation stack using GMapping as SLAM algorithm I was curious to make tests with Hector Slam that, as shown by RoboPeak on its youtube video, works really well.

I registered real sensor data (using rosbag) from Geduino and play it running GMapping and Hector Slam in order to compare their behaviour on same data. Those are the results:

The map generated by GMapping

The map generated by Hector Slam

I was really surprised of this result: Hector Slam performs really better than GMapping!

Those two algorithms differ from the information sources used for localization and mapping: GMapping uses odometry and laser scan; Hector Slam, instead, uses the laser scan only. Theoretically GMapping should perform better then Hector Slam expecially on environments that cause laser scan estimated pose to be ambiguous (large space or long hallway without features): in those scenario GMapping can rely on odometry for robot localization. By other hand Hector Slam does not require odometry (so its a forced choice if robot does not provide it); another big advantage is that Hector Slam can work with laser mounted not planar to ground (as required by GMapping).

You can find more info on this benchmark of slam algorithm in ROS.

Since Hector Slam works fine we can assume laser scan data is ok, so I focused on odometry test: a good guide for this purpose can be found here.

This the result of this test:

The RViz output of odometry test

The robot starts from the position shown on the picture and travel to other side of the room and gone back. As shown on the picture laser scan impressions overlap quite well: this means that odometry data is consistent.

In conclusion theory is not according with real world: GMapping should perform better than Hector Slam but tests demonstrate the opposite. I will spend some time in order to study this case and found a solution since Geduino is designed to use GMapping.