Multi-Hypothesis AMCL (MH-AMCL)

MH-AMCL is a fully functional localization algorithm implementation with Nav2. The main feature is that it maintains several hypotheses about the robot’s position. The algorithm periodically generates new hypotheses on positions where the robot could be, based on the latest laser and map reading. This allows:

• Total unknown the position of the robot.
• Recover from erroneous estimates and hijacks.

Build

This package requires the Rolling distribution, as it is in sync with Nav2, whose main branch uses Rolling.

Just clone this repo in the workspace and build as usual:

colcon build --symlink-install

Run

We have included in this package launchers and other files that are usually in the nav2_bringup package in order to have a demo of its operation:

• To run in the simulation with a Turtlebot 3: ros2 launch mh_amcl tb3_simulation_launch.py

• To run in the simulation with a real robot (Tiago):

  • ros2 launch mh_amcl tiago_launch.py
  • If you don’t have the robot, you can launch a demo ros2 bag with real data below: ros2 bag play test/rosbag2_2022_09_01-11_42_10

Details

Suscribed Topics:

• scan (sensor_msgs/msg/LaserScan): Laser readings.
• map (nav_msgs/msg/OccupancyGrid): The environmen map.
• initialpose (geometry_msgs/msg/PoseWithCovarianceStamped): Used for reset the robot’s position from Rviz2.

Published Topics:

• amcl_pose (geometry_msgs::msg::PoseWithCovarianceStamped): The robot’s pose with the covariance associated from the best hypothesis.
• particle_cloud (nav2_msgs::msg::ParticleCloud): The particles from the best hypothesis.
• poses (visualization_msgs::msg::MarkerArray): All the particles from all the hypotheses, each one with a different color.

Parameters:

• use_sim_time (bool, False): Use the robot’s clock or the one coming from the /clock topic.
• max_particles (int, 200): The maximum number of particles for each hypothesis.
• min_particles (int, 200): The minimum number of particles for each hypothesis.
• particles_step (int, 30): Particles’ variation increases particles_step when the estimation is bad and decreases when it is good.
• init_pos_x (double): The initial X position of the robot, if known.
• init_pos_y (double): The initial Y position of the robot, if known.
• init_pos_yaw (double): The initial Yaw position of the robot, if known.
• init_error_x (double): The initial X uncertainty of the robot.
• init_error_y (double): The initial Y uncertainty of the robot.
• init_error_yaw (double): The initial Yaw uncertainty of the robot.
• translation_noise (double, 10%): The error percentage coming from the translation component.
• rotation_noise (double, 10%): The error percentage from the rotational component.
• rotation_noise (double, 10%): The error percentage from the rotational component.
• distance_perception_error (double, 0.01): The error in meters of the sensor when reading distances.
• reseed_percentage_losers (double, 90%): The percentage of particles to be replaced when reseeding.
• reseed_percentage_winners (double, 3%): The percentage of particles that generate new particles when reseeding.
• multihypothesis (bool, true): Use multiples hypothesis, or only one - the created initially.
• max_hypotheses (int, 5): Maximum number of concurrent hypotheses.
• min_candidate_weight (float, 0.5): Minimum quality of a candidate to be considered for a new hypothesis.
• min_candidate_distance (double, 1.0): Minimum distance to an existing hypothesis to be considered for a new hypothesis.
• min_candidate_angle (double, PI/2): Minimum angle to an existing hypothesis to be considered for a new hypothesis.
• low_q_hypo_thereshold (float, 0.25): Under this threshold, a hypothesis is considered low quality and should be removed if there is a better candidate.
• very_low_q_hypo_thereshold (float, 0.10): A hypothesis is considered very low quality and should be removed under this threshold.
• hypo_merge_distance (double, 0.3): Distance under consideration to merge two hypotesese (angle and distance shpuld meet).
• hypo_merge_angle (double, 0.5): Angle to consider merging two hypotheses (angle and distance should meet).
• good_hypo_thereshold double, 0.5): Threshold to consider a hypothesis to be selected as the newly selected hypothesis for the algorithm’s output.
• min_hypo_diff_winner double, 0.3): An hypothesis should have a quality min_hypo_diff_winner better than the currently selected hypothesis to be the newly selected hypothesis. Low values could lead to continuing changes between the two hypotheses. High values could make it impossible to consider other hypotheses.

Citing

Coming soon! (we hope)

Contribution Guidelines

As an open-source project, we welcome and encourage the community to submit patches directly to the Multi-Hypotesis AMCL(MH-AMCL). In our collaborative open source environment, standards and methods for submitting changes help reduce the chaos that can result from an active development community.

This document explains how to participate in project conversations, log and track bugs and enhancement requests, and submit patches to the project so your patch will be accepted quickly in the codebase.

Licensing Licensing is very important to open source projects. It helps ensure the software continues to be available under the terms that the author desired.

Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that license:

5. Submission of Contributions. Unless You explicitly state otherwise,
   any Contribution intentionally submitted for inclusion in the Work
   by You to the Licensor shall be under the terms and conditions of
   this License, without any additional terms or conditions.
   Notwithstanding the above, nothing herein shall supersede or modify
   the terms of any separate license agreement you may have executed
   with Licensor regarding such Contributions.

A license tells you what rights you have as a developer, as provided by the copyright holder. It is important that the contributor fully understands the licensing rights and agrees to them. Sometimes the copyright holder isn’t the contributor, such as when the contributor is doing work on behalf of a company.

Developer Certification of Origin (DCO)

To make a good faith effort to ensure licensing criteria are met,MH-AMCL requires the Developer Certificate of Origin (DCO) process to be followed.

The DCO is an attestation attached to every contribution made by every developer. In the commit message of the contribution, (described more fully later in this document), the developer simply adds a Signed-off-by statement and thereby agrees to the DCO.

When a developer submits a patch, it is a commitment that the contributor has the right to submit the patch per the license. The DCO agreement is shown below and at http://developercertificate.org/.

Developer's Certificate of Origin 1.1

By making a contribution to this project, I certify that:

(a) The contribution was created in whole or in part by me and I
    have the right to submit it under the open source license
    indicated in the file; or

(b) The contribution is based upon previous work that, to the
    best of my knowledge, is covered under an appropriate open
    source license and I have the right under that license to
    submit that work with modifications, whether created in whole
    or in part by me, under the same open source license (unless
    I am permitted to submit under a different license), as
    Indicated in the file; or

(c) The contribution was provided directly to me by some other
    person who certified (a), (b) or (c) and I have not modified
    it.

(d) I understand and agree that this project and the contribution
    are public and that a record of the contribution (including
    all personal information I submit with it, including my
    sign-off) is maintained indefinitely and may be redistributed
    consistent with this project or the open source license(s)
    involved.

DCO Sign-Off Methods

The DCO requires that a sign-off message, in the following format, appears on each commit in the pull request:

Signed-off-by: Sofforus Jones <sjones@gmail.com>

The DCO text can either be manually added to your commit body, or you can add either -s or --signoff to your usual Git commit commands. If you forget to add the sign-off you can also amend a previous commit with the sign-off by running git commit --amend -s. If you’ve pushed your changes to GitHub already you’ll need to force push your branch after this with git push -f.

Note: The name and email address of the account you use to submit your PR must match the name and email address on the Signed-off-by line in your commit message.