LSM Localization for ROS

Overview

This package implements a localization algorithm that can be used with ROS Navigation as an alternative to the popular AMCL.

In the core of the algorithm is the Kalman filter that fuses the odometry with LIDAR scan measurements. The twist (velocity) part of the odometry topic is integrated in the SE(2) space to calculate the prediction for the Kalman filter. Measurement is produced by first constructing the expected LIDAR image that the robot would see at its previous pose, given the map, followed by calculating the incremental transform that would bring the constructed image into the alignment with the actual LIDAR scan image. The incremental transform is applied to the previous robot pose to produce the measured pose, which updates the Kalman filter state. The incremental pose is calculated using the PL-ICP algorithm from Canonical Scan Matcher package and has been derived from the Laser Scan Matcher package.

Installation

To install the package, clone this repository into your workspace and run catkin_make. You need CSM library which you can build from source using the script provided in this misc directory of this repository

cd misc
sudo ./install_csm <ROS distro>

Parameter <ROS distro> above is the ROS distribution you are building for (e.g. melodic, noetic).

Usage

You can start the LSM Localization node using the launch files provided in this repository. The stand-alone lsm_localization.launch has a sane and robust set of parameters that will work across many environments. You can start the node as follows:

roslaunch lsm_localization lsm_localization.launch

The node will expect map, odometry, and LIDAR scan on standard topics (/map, /scan and /odom respectively) and it will publish the result on lsm_localization/pose topic. Frame names are standard map for fixed frame and base_link for robot base. Other frames (e.g. odometry, LIDAR) are deduced from the messages and the transform tree. The initial pose must be sent on initialpose topic to bootstrap the localization.

For localization to work correctly, you must make sure that odometry is publishing valid covariance in the velocity (twist) section of the odometry message. A common mistake is to publish velocity with zero covariance, in which case the Kalman filter will give all weight to odometry and the output will simply become the replica of odometry.

To save the computation, the node will downsample the LIDAR scan and the downsample rate is set to 4 in the above example launch file. Further the node will only publish the output pose when PL-ICP matching succeeds. So the output pose will not be continuous and it rate will be at most the LIDAR scan rate divided by the downsampling rate.

If you need continuous tracking, you must enable publishing of map-odometry transformation and make sure that your odometry is continuous. Very often poorly designed and or buggy odometry may cause the appearance that the localization is not working, so make sure your odometry is continuous and presents valid covariance.

For a more canned example, you can run

roslaunch lsm_localization lsm_localization_jackal_example.launch

For this example to work, you must install jackal_gazebo and jackal_navigation packages. The launch file will start the simulation if Clearpath Jackal robot on a racetrack along with along with the map server and LSM Localization node. The node will be configured to subscribe to topics that the Jackal simulation package publishes and it will publish map-odometry transformation. It will also take the initial pose from /initialpose topic, so you can bootstrap it from RVIZ. An example configuration for RVIZ is available in misc directory in this repository.

You can give it the initial pose that approximately matches the robot position and orientation and watch it converge. Unlike most implementations of particle filters, LSM Localization runs even if the robot is not moving, so after giving it a reasonably close initial pose, you can watch it converge without moving the robot.

Subscribed topics

File truncated at 100 lines see the full file