Initialize particle filter with last known estimate (#185)

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.

* Initialize filter with last known estimate when we have a map update
* Warn when callback messages are ignored
* Remove `this` usage for consistency
* Remove unused members of `AmclNode`
* Change 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

Signed-off-by: Nahuel Espinosa <[email protected]>

beluga_amcl/include/beluga_amcl/private/amcl_node.hpp

@@ -22,6 +22,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
+#include <utility>
 
 #include <beluga/localization.hpp>
 #include <bondcpp/bond.hpp>
@@ -63,7 +64,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
-  void reinitialize_with_pose(const Eigen::Vector3d & mean, const Eigen::Matrix3d & covariance);
+  void reinitialize_with_pose(const Sophus::SE2d & pose, const Eigen::Matrix3d & covariance);
 
   std::unique_ptr<beluga::LaserLocalizationInterface2d> particle_filter_;
   execution::Policy execution_policy_;
@@ -73,8 +74,6 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
     particle_cloud_pub_;
-  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr
-    likelihood_field_pub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     pose_pub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
@@ -90,7 +89,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
     rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
   message_filters::Connection laser_scan_connection_;
 
-  Sophus::SE2d last_odom_to_base_transform_;
+  std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_;
 
   bool enable_tf_broadcast_{false};
 };