Implement nav2_amcl functionality

[nahueespinosa]

As discussed in our sync, our first goal would be to implement the functionality existing in nav2_amcl/QuickMCL. This will simultaneously give us a concrete target to aim for and a baseline to compare against.

That includes:

• An adaptive particle filter with KLD resampling and a random particle generation heuristic to recover from failure (see https://github.com/ekumenlabs/RnD/discussions/126#discussioncomment-3909891).
• Motion models:
  • Differential drive
  • Omnidirectional
• Sensor models:
  • Likelihood field
  • Beam
• ROS2 interface:
  • Lifecycle Node
  • Parameters
  • Messages
    • http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/LaserScan.html
    • http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html
  • Services
    • http://docs.ros.org/en/api/nav_msgs/html/srv/SetMap.html

The overall goal is to keep interfaces out of core functionality and keep the code generic enough to be extensible.

[nahueespinosa]

Currently, we have a package skeleton with an adaptive particle filter that's intended to be a drop-in replacement of nav2_amcl.
In order to reach our first goal, at high level, the plan of record would be:

• Add a simulation to beluga_example.
• Implement concrete motion and a sensor models.
  • Likelihood field sensor model.
  • Differential drive motion model.
• State representation #7 Use a vector representation for the particle state updating our models accordingly.
• Implement state estimation mixin for the particle filter.
• Integrate everything with the ROS node.
• Parallelize our sensor model.

[nahueespinosa]

So, now that the particle filter is working, what's next?

Here are some things not yet re-implemented from nav2_amcl plus some ideas for improvements and next steps.

• beluga_amcl

  • Provide convertion and transform utilities for the sophus library. See tf2_eigen.
  • Publish map-to-odom transform #40
  • Add support for setting the initial pose #41
  • Periodically save the pose estimate to the parameter server in order to initialize the filter with the last known estimate if the node is restarted.

• beluga

  • Parallelize motion model updates (should be trivial).
  • Investigate beam sensor model #54
  • Investigate omnidirectional motion model #56
  • Reduce the resampling frequency #57
  • Consider orientation when computing the spatial hash #68
  • Compute the estimate based on the most likely cluster?
  • Use abseil improved random algorithms. Note: They are faster in the benchmarks, but the resampling step takes about the same time since the bottleneck is elsewhere. It's not worth adding an extra dependency IMO.

• others

  • Compare performance between beluga_amcl and nav2_amcl #35
  • Evaluate using real datasets instead of simulation.
  • Run pre-commit hooks on CI.

[nahueespinosa]

Let's close this in favor of #85.