Make beluga_amcl a drop-in replacement of nav2_amcl

[nahueespinosa]

Description

This ticket is meant to track the progress in making a drop-in replacement of nav2_amcl in terms of interface and parameters.
Specific features and requirements will be best tracked in their own issues.

Definition of done

Complete the implementation of the following list of parameters, services, publishers and subscribers:

• Parameters
  • ROS topics and frames
    • base_frame_id
    • global_frame_id
    • odom_frame_id
    • scan_topic
    • map_topic
    • initial_pose_topic (extra)
  • Initial pose (see Add support for setting the initial pose #41)
    • set_initial_pose
    • initial_pose
    • save_pose_rate
    • always_reset_initial_pose
    • first_map_only
  • Publish map to odom transform (see Publish map-to-odom transform #40)
    • tf_broadcast
    • transform_tolerance
  • Particle filter
    • max_particles
    • min_particles
    • pf_err
    • pf_z
    • recovery_alpha_fast
    • recovery_alpha_slow
    • spatial_resolution (extra)
    • resample_interval (see Reduce the resampling frequency #57)
    • selective_resampling (see Reduce the resampling frequency #57)
  • Motion model
    • robot_model_type
    • alpha1
    • alpha2
    • alpha3
    • alpha4
    • alpha5 (see Investigate omnidirectional motion model #56)
    • update_min_a
    • update_min_d
  • Sensor model
    • laser_model_type
    • laser_max_range
    • laser_min_range
    • max_beams (see Add max_beams parameter to beluga_amcl #84)
    • sigma_hit
    • z_hit
    • z_rand
  • Likelihood field model exclusive
    • laser_likelihood_max_dist
    • beam_skip_distance
    • beam_skip_error_threshold
    • beam_skip_threshold
    • do_beamskip (see Beam skipping in likelihood field model #187)
  • Beam model exclusive (see Investigate beam sensor model #54)
    • lambda_short
    • z_max
    • z_short
• Services
  • reinitialize_global_localization
  • request_nomotion_update (see Force update when there is no movement #184)
• Publishers
  • pose
  • particle_cloud
  • likelihood_field (extra)
• Subscribers
  • map
  • initial_pose
  • scan

[glpuga]

I just noticed that that ROS1 AMCL has a selective_resampling option that's not present in NAV2. This is different from the resample_interval option. This may be relevant if we decide that #95 is worth pursuing. Apparently it implements this paper:

Grisetti, Giorgio, Cyrill Stachniss, and Wolfram Burgard. "Improved techniques for grid mapping with rao-blackwellized particle filters."

It's a feature that was merged into Kinetic early in 2020, was then forwarded to Melodic, and is present in Noetic. However, it seems to have fallen through the cracks before it made it to NAV2 (this may be worth bringing up as well).

[nahueespinosa]

Good find! I added a bullet point for this in #57.

[olmerg]

The two services implemented in nav2 are reinitialize_global_localization and request_nomotion_update , this service do not receive or return none, just generate an action inside the node:

• Reinitialize global localization call pf_init_model and set initial_pose_is_known by true. This service Initialize the filter using some model which consists in create kd-tree for adaptive sampling, computing the new samples poses, adding samples to the histogram and re-compute cluster statistics. All of these use a uniform Pose generator over the map. It is like create again the object so I think we can use ParticleBaselineGeneration.
• Request no motion update put in true force_update_. this variable modifies if we should update the filter. Additionally, when this flag is true the particle cloud is not published. Some additional point about this flag is that set to true on_cleanup and set false after it is used to decide read the laser. So conclusion this service forces the filter to work when there is no movement.

[olmerg]

About parameters:

• save_pose_rate parameter (0.5). Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables initial_pose_ and initial_cov. This saved pose will be used on subsequent runs to initialize the filter (-1.0 to disable). This creates a variable called save_pose_period_ but this variable I do not find where is used. I do not find where initial_pose and initial_cov are used.
• always_reset_initial_pose (false). Requires that AMCL is provided an initial pose either via topic or initial_pose parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize. This variable set initial_pose_is_known_ like false, which only can change by initial_pose topic or reinitialize_global_localization
• first_map_only (false). Set this to true, when you want to load a new map published from the map_server. Just not update the map when the callback receives a new map.

[olmerg]

About todo in amcl_node,

• TODO(ivanpauno): Intialize later maps from last known pose.
• TODO(nahuel): Add a parameter for the timer period.
• TODO(nahuel): Throttle the particle cloud publishing in the laser_callback method instead of using a ROS timer.
• do we have ? When pausing and resuming, remember the last robot pose so we don't start at 0:0 again

[olmerg]

• save_pose_rate parameter (0.5). Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables initial_pose_ and initial_cov. This saved pose will be used on subsequent runs to initialize the filter (-1.0 to disable). This creates a variable called save_pose_period_ but this variable I do not find where is used. I do not find where initial_pose and initial_cov are used.

This parameter come from ros 1 version the idea was "Is it time to save our last pose to the param server"
https://github.com/ros-planning/navigation/blob/noetic-devel/amcl/src/amcl_node.cpp#L1518

In ros2 the action to save the parameters is made during on_cleanup

[olmerg]

About parameters:

• save_pose_rate parameter (0.5). Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables initial_pose_ and initial_cov. This saved pose will be used on subsequent runs to initialize the filter (-1.0 to disable). This creates a variable called save_pose_period_ but this variable I do not find where is used. I do not find where initial_pose and initial_cov are used.

This parameter is deprecated!!

[nahueespinosa]

With #196 documenting the current compatibility state between beluga_amcl and nav2_amcl, this ticket is no longer necessary.

#172, #184 and #187 describe the remaining work to be done to achieve full feature parity.