mrpt_pf_localization

• mrpt_pf_localization
  • Overview
  • Related papers
  • Configuration
  • Metric map conceptual model
  • Demos
    • 2D LIDAR localization with a gridmap
    • Range-only (RO) localization with a set of fixed, known radio beacons
  • Node: mrpt_pf_localization
    • Working rationale
    • ROS 2 parameters
    • Subscribed topics
    • Published topics
    • Template ROS 2 launch files

Overview

This package provides a ROS 2 node for self-localization using 2D or 3D (SE(2) or SE(3)) particle filter-based algorithms and a number of different metric maps as reference maps to which to "compare" sensor observations.

In a sense, this package is an equivalent to the classic ROS 1 amcl, but with superpowers :-)

Features:

• A number of different PF algorithms.

• Different map types: Occupancy grid maps (as images, ROS yaml files, or in MRPT binary format), point clouds, beacon map (for range-only sensors). At present, these combinations are exposed in this node:

  • Map: occupancy grid, Sensor: anyone capable of generating a point cloud. Several occupancy grids, each at a different height, to be used for laser scans at the corresponding robot height.
  • Map: beacons at predefined 3D positions, Sensor: range-only. For Range-Only (RO) Localization.
  • Map: point cloud, Sensor: 2D or 3D Lidars (TO-DO as of Aug 2023).
  • GNSS (GPS) readings, in parallel to any of the above (TO-DO as of Aug 2023).

• Multiple simultaneous sensors: The combinations above can be used together and their probabilistic information automatically fused together.

Related papers

• Optimal particle filtering algorithm:

  J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "Optimal Filtering for Non-Parametric Observation Models: Applications to Localization and SLAM", The International Journal of Robotics Research (IJRR), vol. 29, no. 14, 2010. (PDF)

• Range-Only localization:

  J. Gonzalez-Jimenez, J.L. Blanco, C. Galindo, A. Ortiz-de-Galisteo, J.A. Fernandez-Madrigal, F.A. Moreno, J. Martinez, "Mobile Robot Localization based on Ultra-Wide-Band Ranging: A Particle Filter Approach", Robotics and Autonomous Systems, vol. 57, no. 5, pp. 496--507, 2009. (PDF)

Configuration

The provided algorithms have parameters that can be grouped into three conceptual topics:

• Algorithm: Parameters affecting the particle filter itself or the adaptive sampling method. These parameters can be set in the main config YAML file.
• Actions: The motion model uncertainty. These parameters are also set in the main config YAML file.
• Observations: These parameters are spread in part in the observations themselves (e.g. each lidar/sonar should carry information about how noisy it is), and the metric maps. The latter are key parameters and in MRPT are called likelihood options in each available metric map.

Metric map conceptual model

Metric map likelihood options are key for tuning the localization system, as they tell how much to "trust" sensor readings, how much to downsample their rays, etc.

When using as input a metric map that comes in MRPT native mrpt::maps::CMetricMap format (this includes mp2p_icp's metric map *.mm files), the map already comes with its own set of likelihood parameters, defined at the time of creating the map in the source application.

However, this mrpt_pf_localization node allows overriding the likelihood options to ease tuning and adjusting without touching the original map.

When using non MRPT-native map sources (e.g. ROS gridmap yaml files), the only way to set these important options is via this overriding mechanism.

Refer to node launch arguments for details.

Demos

2D LIDAR localization with a gridmap and MVSim

Demo video. Run:

ros2 launch mrpt_tutorials demo_localization_pf_mvsim_2d_lidar.launch.py

to start:

• mrpt_pf_localization with the map to be received via a ROS topic,
• mrpt_map_server server loading and publishing a gridmap as reference map via an ROS-styled map.yaml file,
• rviz2 for visualization,
• mvsim to simulate a live robot that can be teleoperated.

Range-only (RO) localization with a set of fixed, known radio beacons

Run:

ros2 launch mrpt_localization demo_ro.launch

to start:

• a dataset (rawlog format) including RO and odometry observations,
• the mrpt localization with known beacon locations, and
• RViz for visualization

Node: mrpt_pf_localization

Working rationale

The C++ ROS 2 node comprises an internal, independent PFLocalizationCore C++ class, which implements the main functionality. It features an internal finite state machine (FSM) with these states:

• UNINITIALIZED: The filter has been neither initialized nor parameters/map loaded. State after initialization. "Loops" in this state do nothing.
• TO_BE_INITIALIZED: Once the parameters have been loaded, and a map has been loaded (or if subscribed to a map topic, the topic data has been received), the PFLocalizationCore is put into this state by the node. Upon next "loop", the particles and data structures will be initialized.
• RUNNING: Normal state. At each "loop", odometry (if present) is used together with sensors to localize the robot.

ROS 2 parameters

There is a core set of parameters to configure the particle filter algorithm itself, which is self-documented in the provided template parameters yaml file: params/default.config.yaml. Please, read that file and its comments for details.

If the initial_pose parameter is provided, and there is an occupancy gridmap, particles will be distributed along free space cells only. Otherwise, they will be distributed in the box that circumscribes the confidence interval of mean ±1 sigma of the uncertainty.

Subscribed topics

• xxx

Published topics

• xxx

Template ROS 2 launch files

This package provides launch/localization.launch.py:

ros2 launch mrpt_pf_localization localization.launch.py

which can be used in user projects to launch the MRPT PF localization node, by setting these launch arguments:

• mrpt_map_config_file: Path to an INI file with metric map (mrpt::maps::CMetricMap) parameters to override the incoming map likelihoodOptions. If this optional parameter is not provided, likelihood options will be taken from the map sent by the mrpt_map_server node.
• pf_params_file (Default: params/default.config.yaml): If defined, overrides the default particle filter algorithm. Note that probabilistic likelihood models for the metric maps are in an independent file (mrpt_map_config_file).

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package mrpt_pf_localization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

1.0.3 (2022-06-25)

1.0.2 (2022-06-25)

1.0.1 (2022-06-24)

• fix all build errors; removed now obsolete tf_prefix
• Ported to tf2 and mrpt::ros1bridge
• modernize cmake
• Contributors: Jose Luis Blanco-Claraco

1.0.0 (2022-04-30)

• Update URLs to https
• Update build dep to mrpt2
• Merge pull request #118 from mx-robotics/ag-remove_laser_tf_cache continuous laser pose update from tf tree fag added
• added ROS parameter continuous sensor pose update
• Fix incorrect publication of estimated pose (Closes: #117)
• Contributors: Jose Luis Blanco Claraco, Markus Bader

0.1.26 (2019-10-05)

0.1.25 (2019-10-04)

• fix build against mrpt2
• Contributors: Jose Luis Blanco-Claraco

0.1.24 (2019-04-12)

• Fix build against MRPT 1.9.9
• Contributors: Inounx, Jose Luis Blanco-Claraco, Julian Lopez Velasquez, Markus Bader

0.1.23 (2018-06-14)

0.1.22 (2018-05-22)

• fix all catkin_lint errors
• remove exec +x flag to cfg files
• Contributors: Jose Luis Blanco-Claraco

0.1.21 (2018-04-27)

• Upgrade version 0.1.20 (#99)
• fix build against mrpt 2.0
• partial fix build w mrpt 2.0
• fix build in mrpt 2.0
• optimized build (-O3)
• Fix travis (#94)
  • add dep stereo_msgs
  • add dep stereo_msgs
  • fix minor warnigngs and errors
• fix use c++14
• Merge branch \'master\' of github.com:tuw-robotics/mrpt_navigation
• Merge branch \'master\' into master
• CMake finds MRPT >=1.5 in ROS master branch
• Merge branch \'master\' into compat-mrpt-1.5
• CMake finds MRPT >=1.9
• avoid Eigen warnings with GCC-7
• Removed unnecessry MRPT_VERSION checks
• Fixes for clang format
• Removed c++11 declarations in cmake
• Adapted CMakeLists to new mrpt
• Ported to a new version of MRPT
• Merge pull request #74 from bergercookie/devel Use C++11 in mrpt_bridge, mrpt_pf_localization
• Merge pull request #77 from corot/master Allow using maps from topic
• Allow using maps from topic
• Use C++11 in mrpt_bridge, mrpt_pf_localization
• Merge pull request #72 from bergercookie/devel Add more conversion methods for CNetworkOfPoses classes
• Correct include guard

• Fix debug messages: (#65)

  • replace printf with log_info
  • read and use debug param

  * dumping to console only once Also publish pose even if not updating the filter (sorry for mixing commits)

• Fix debug messages:

  • replace printf with log_info
  • read and use debug param

  * dumping to console only once Also publish pose even if not updating the filter (sorry for mixing commits)

• put right the covariance matrix initialization (#63)

• Merge pull request #61 from corot/master Prevent extrapolation into the past when publishing the tf

• Fix a bug in the way I handle filter state

• Prevent extrapolation into the past when publishing the tf and handle update/not update more nicelly. Also, ROS-format variables

• Contributors: Ashish Raste, Borys Tymchenko, Hunter Laux, Jorge Santos, Jorge Santos Sim