Initialize particle filter with last known estimate

[nahueespinosa]

Related to #183. Now the particle filter will use the last known estimate for initialization when a new map arrives. Ideally we would update the occupancy grid in the particle filter without losing the current particle set, but that feature doesn't exist in Beluga yet.
Related to #85.

Summary

• Initialize particle filter with last known estimate when we have a map update
• Warn when callbacks are ignored
• Remove this usage for consistency
• Remove unused member odom_to_base_transform_
• Change default configuration file to play nicely with the example rosbag
• Add always_reset_initial_pose parameter
• Add first_map_only parameter
• Add mutually exclusive callback group

Checklist

• Read the contributing guidelines.
• Configured pre-commit and ran colcon test locally.
• Signed all commits for DCO.
• Added tests (regression tests for bugs, coverage of new code for features).
• Updated documentation (as needed).
• Checked that CI is passing.

[hidmic]

Is there a way we can validate this functionality in system tests?

[nahueespinosa]

Is there a way we can validate this functionality in system tests?

Those are system tests for the Beluga library. It re-uses Beluga AMCL code, but I hope we can break that dependency at some point. We need to add tests for this node in this package. That's why #101 is still open.

[glpuga]

(I've not seen the code)

Probably reinitializing with the last known estimate is a thing we can do, but I wonder about the usefulness of any type of reinitialization here.

The map could be changed to anything, with little or relation to the previous map, so the previous estimation (in the previous map frame of reference) may be totally random in the new map if the frame changed.

For instance, the one case I know of updating maps while in operation is one of our clients management of lifts. The robot would take the lift, and update the map between floors. In this case, the previous estimate would usually be totally unrelated to the pose of the robot in the new map, so they would have to set the pose as well in sequence.

In the lift example above things would work regardless if we just assume a pose (e.g. the previous estimate) and then the filter estimation is externally set. However, there's a small chance that the glitchy interval between us resetting the previous estimate and the mission control setting a new pose within the updated map might trigger other processes in unfortunate cases (for instance, an alarm because the robot is thought to be close to some dangerous area during the glitch).

I wonder if we should estimate at all if we have a brand new map but no initial pose in it.

[hidmic]

Probably reinitializing with the last known estimate is a thing we can do, but I wonder about the usefulness of any type of reinitialization here.

IMO it's completely useless except during node cycling (deactivate, reactivate). I also agree that in most cases you want the filter to be externally initialized. We still need it for feature parity though.

there's a small chance that the glitchy interval between us resetting the previous estimate and the mission control setting a new pose within the updated map might trigger other processes in unfortunate cases (for instance, an alarm because the robot is thought to be close to some dangerous area during the glitch).

Indeed it would be nice for the node to wait for pose initialization upon map update, perhaps by default.

[glpuga]

We still need it for feature parity though.

+1

Indeed it would be nice for the node to wait for pose initialization upon map update, perhaps by default.

Maybe in the future we can add a parameter to enable this and probably other "sane" behaviors. We would default to full AMCL compatibility, but you can set it to true if you feel like it.

[nahueespinosa]

Somewhat related to the conversation, I added a check to verify that the map frame is the global frame (also done in nav2_amcl).
I guess according to REP 105, map is a world fixed frame.

We still need it for feature parity though.

+1

Indeed it would be nice for the node to wait for pose initialization upon map update, perhaps by default.

That can be achieved with the set_initial_pose: false since it will wait for the initial pose topic message. Nevermind.

Maybe in the future we can add a parameter to enable this and probably other "sane" behaviors.

Sounds good! Or we can keep this one and build a new ROS node with the lessons learned.