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:

• pf_params_file (Default: params/default.config.yaml): If defined, overrides the default particle filter algorithm.

Changelog for package mrpt_pf_localization

2.2.1 (2024-10-12)

2.2.0 (2024-09-25)

• less strict unit test failure limit (fixes potential spurious failure)
• Update URL entries in package.xml to each package proper documentation
• ament linters: manually enable just cmake and xml linters
• reformat clang-format with 100 column width
• mrpt_pf_localization is now robust against temporary failures of /tf resolutions for sensor poses
• New optional param 'metric_map_use_only_these_layers' to use only a subset of the .mm map layers
• Contributors: Jose Luis Blanco-Claraco

2.1.1 (2024-09-02)

• Remove temporary workaround in <depends> for buggy mrpt_libros_bridge package.xml
• update dependencies
• Depend on new mrpt_lib packages (deprecate mrpt2)
• Contributors: Jose Luis Blanco-Claraco
• Remove temporary workaround in <depends> for buggy mrpt_libros_bridge package.xml
• update dependencies
• Depend on new mrpt_lib packages (deprecate mrpt2)
• Contributors: Jose Luis Blanco-Claraco

2.1.0 (2024-08-08)

• Fix GNSS name typo
• Merge branch 'ros2' into wip/port-tps-astar
• Merge branch 'ros2' into wip/port-tps-astar
• Contributors: Jose Luis Blanco-Claraco

2.0.1 (2024-05-28)

2.0.0 (2024-05-28)

• unit test executable now accepts many env var arguments for use in batch tests
• More relocalization parameters
• pf-test: fix static not POD warnings, and support env var-based config file too
• Fix for latest mp2p_icp api
• Implement relocalization based on ICP
• fix relocalization with reference pointmaps
• code clean up; check convergence in unit test
• Do not update the PF if there are no usable observations
• use pf/m² to initialize; estimate twist
• Expose gnss topic in the launch file
• GNSS-based initialization
• Comply with ROS2 REP-2003
• use namespaces for launch files
• allow overriding map likelihood options
• Show more debug info on metric map likelihood options
• New param for core-only debug level
• enable running the test from an MM file
• Receive gridmap from ROS topic
• Clearer warn messages
• Reorganize launch and demo files
• Prepare demo launch files
• Port mrpt localization to ROS 2 and whole refactor
• Renamed: mrpt_localization: mrpt_pf_localization
• Contributors: Jose Luis Blanco-Claraco

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ón, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Markus Bader, Nikos Koukis, corot

0.1.20 (2018-04-26)

• 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, Jorge Santos, Jorge Santos Simón, Jose Luis Blanco Claraco, Jose Luis Blanco-Claraco, Markus Bader, Nikos Koukis, corot

0.1.18 (2017-01-22)

0.1.17 (2017-01-22)

• Do not consider epsilon velocities (<1e-3) as robot moving
• make catkin_lint clean
• Remove all errors generated by catkin_lint and cleanup unused templates from CMakeLists.txt files
• Issue #53: add a parameter to disable updating when robot not moving
• Contributors: Jorge Santos, Jose Luis Blanco

0.1.16 (2016-12-13)

• Fix for issue #50
• Tabs to spaces
• Fix for issue #48
• Remove unneeded include
• Allow robot poses from external algorithms to be integrated into mrpt particles filter
• fix typo
• Contributors: Jorge Santos, Jorge Santos Simón, Jose-Luis Blanco-Claraco

0.1.15 (2016-11-06)

• Fix build against MRPT 1.5.0
• Use ros::Time::now() to time stamp first 10 poses If not, they contain wall time, what when working on simulation prevents robot_localization fusion to work. Other than that, the change is innocuous
• PR #33 prevented pose initialization with the robot stopped; fix it!
• Stop mrpt_pf_localization updating when robot is not moving (odom twist is zero)
• Contributors: Jorge Santos, Jorge Santos Simón, Jose-Luis Blanco-Claraco

0.1.14 (2016-09-12)

0.1.13 (2016-09-03)

0.1.12 (2016-09-03)

• Put the ROS log setting withing if MRPT_VERSION>=0x150 so it doesn't break the compilation agains .deb mrpt libs
• Restamp pose on first iteration with ROS time because filter time is still not initialized and can create problems when integrating on robot_localization
• Set ROS log level also on MRPT internal log system. Prevents spamming of [FIXED_SAMPLING] and [ADAPTIVE SAMPLE SIZE] messages
• Modify so we can use in conjuntion with robot_localization package: provide a PoseWithCovarianceStamped, allow disabling tf publishing and make transform_tolerance a parameter
• Contributors: Jorge Santos

0.1.11 (2016-08-21)

0.1.10 (2016-08-05)

0.1.9 (2016-08-05)

0.1.8 (2016-06-29)

0.1.7 (2016-06-20)

• Fix laser scan stamp problem. TODO: something is still broken since nothing pops up for mrpt_pose
• fix almost everything to add a pose publisher
• Contributors: Megacephalo

0.1.6 (2016-03-20)

• New support for range-only (RO) localization
• fix build against mrpt <1.3.0
• Contributors: Jose Luis Blanco, Jose Luis Blanco-Claraco, Raphael Zack

0.1.5 (2015-04-29)

• fix to strange pf-localization bug
• Cleaner build against mrpt 1.3.0
• Fix build against mrpt 1.3.0
• Contributors: Jose Luis Blanco

0.1.4 (2014-12-27)

• dont publish if numSubscribers()==0
• fixes for mrpt 1.3.0
• Removed 'mrpt' dep from catkin_package(). I think this is giving problems to dependant pkgs and is not needed...
• pose_cov_ops removed from mrpt_navigation metapkg
• localization: New param to configure sensor sources in a flexible way
• Contributors: Jose Luis Blanco

0.1.3 (2014-12-18)

• Fix many missing install files
• Contributors: Jose Luis Blanco

0.1.2 (2014-12-18)

0.1.1 (2014-12-17)

• First public binary release.

0.1.0 (2014-12-17)

• consistent version numbers
• fix build error without WX
• Fixes broken dependencies
• config and demos tested
• localization working like amcl