Add timer-based propagation functionality to AMCL

beluga_amcl/README.md

@@ -32,12 +32,13 @@ See [Beluga AMCL documentation](https://ekumen-os.github.io/beluga/packages/belu
 
 ### Subscribed Topics
 
-The subscribed topic names can be changed with the parameters `map_topic`, `scan_topic` and `initial_pose_topic`.
+The subscribed topic names can be changed with the parameters `map_topic`, `scan_topic`, `odom_topic` and `initial_pose_topic`.
 
 | Topic            | Type                                      | Description                                                                 |
 |------------------|-------------------------------------------|-----------------------------------------------------------------------------|
 | `map`            | `nav_msgs/OccupancyGrid`                  | Input topic for map updates.                                                |
 | `scan`           | `sensor_msgs/LaserScan`                   | Input topic for laser scan updates.                                         |
+| `odom`           | `nav_msgs/Odometry`                       | Input topic for odometry updates (when use_odometry_propagation is enabled).       |
 | `initial_pose`   | `geometry_msgs/PoseWithCovarianceStamped` | Input topic for pose mean and covariance to initialize the particle filter. |
 
 ### Published Topics

beluga_amcl/config/Amcl.cfg

@@ -75,6 +75,13 @@ gen.add(
     default="scan"
 )
 
+gen.add(
+    "odom_topic", str_t, 0,
+    "Topic to subscribe to in order to "
+    "receive the odometry data for localization.",
+    default="odom"
+)
+
 gen.add(
     "min_particles", int_t, 0,
     "Minimum allowed number of particles.",

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -40,6 +40,8 @@
 
 #include <beluga/beluga.hpp>
 #include <beluga_ros/amcl.hpp>
+#include <deque>
+#include <nav_msgs/msg/odometry.hpp>
 #include "beluga_amcl/message_filters.hpp"
 #include "beluga_amcl/ros2_common.hpp"
 
@@ -60,6 +62,9 @@ class AmclNode : public BaseAMCLNode {
   ~AmclNode() override;
 
  protected:
+  /// Type Buffer for queued odometry motions (timestamp, pose)
+  using OdometryMotion = std::pair<tf2::TimePoint, Sophus::SE2d>;
+
   /// Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.
   void do_activate(const rclcpp_lifecycle::State&) override;
 
@@ -91,6 +96,12 @@ class AmclNode : public BaseAMCLNode {
   /// Callback for laser scan updates.
   void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr);
 
+  /// Callback for odometry updates.
+  void odometry_callback(nav_msgs::msg::Odometry::ConstSharedPtr);
+
+  /// Processes and removes from the buffer all odometry actions up to a given time point.
+  void process_buffered_odometry_until(std::deque<OdometryMotion>& buffer, const tf2::TimePoint& until);
+
   /// Callback for pose (re)initialization.
   void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override;
 
@@ -143,6 +154,9 @@ class AmclNode : public BaseAMCLNode {
   /// Connection for laser scan updates filter and callback.
   ::message_filters::Connection laser_scan_connection_;
 
+  /// Odometry updates subscription.
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+
   /// Particle filter instance.
   std::unique_ptr<beluga_ros::Amcl> particle_filter_;
   /// Last known pose estimate, if any.
@@ -151,6 +165,8 @@ class AmclNode : public BaseAMCLNode {
   std::optional<Sophus::SE2d> last_known_odom_transform_in_map_;
   /// Whether to broadcast transforms or not.
   bool enable_tf_broadcast_{false};
+  /// Buffer for queued odometry motions (timestamp, pose)
+  std::deque<OdometryMotion> odometry_motion_buffer_;
 };
 
 }  // namespace beluga_amcl

beluga_amcl/src/amcl_node.cpp

@@ -183,6 +183,12 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", ""
         "and ignore subsequent ones.";
     declare_parameter("first_map_only", false, descriptor);
   }
+
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "Set true to enable odometry-driven filter propagation.";
+    declare_parameter("use_odometry_propagation", rclcpp::ParameterValue(false), descriptor);
+  }
 }
 
 AmclNode::~AmclNode() {
@@ -251,6 +257,18 @@ void AmclNode::do_activate(const rclcpp_lifecycle::State&) {
           std::placeholders::_3),
       common_service_qos, common_callback_group_);
   RCLCPP_INFO(get_logger(), "Created request_nomotion_update service");
+
+  {
+    // Subscribe to odometry topic to buffer odometry motions
+    if (get_parameter("use_odometry_propagation").as_bool()) {
+      odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+          get_parameter("odom_topic").as_string(), rclcpp::SensorDataQoS(),
+          std::bind(&AmclNode::odometry_callback, this, std::placeholders::_1), common_subscription_options_);
+
+      RCLCPP_INFO(get_logger(), "Subscribed to odom_topic: %s", odom_sub_->get_topic_name());
+      odometry_motion_buffer_.clear();
+    }
+  }
 }
 
 void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) {
@@ -259,6 +277,7 @@ void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) {
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
   global_localization_server_.reset();
+  odom_sub_.reset();
   if (likelihood_field_pub_) {
     likelihood_field_pub_->on_deactivate();
   }
@@ -268,6 +287,7 @@ void AmclNode::do_cleanup(const rclcpp_lifecycle::State&) {
   particle_filter_.reset();
   likelihood_field_pub_.reset();  // attaching likelihood_field_pub_ lifespan to particle_filter_ lifespan
   enable_tf_broadcast_ = false;
+  odometry_motion_buffer_.clear();
 }
 
 auto AmclNode::get_initial_estimate() const -> std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> {
@@ -466,13 +486,45 @@ void AmclNode::do_periodic_timer_callback() {
   }
 }
 
+void AmclNode::odometry_callback(nav_msgs::msg::Odometry::ConstSharedPtr odom) {
+  if (!particle_filter_) {
+    RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 2000,
+        "Ignoring odometry data because the particle filter has not been initialized");
+    return;
+  }
+  // Use the odometry message timestamp and pose
+  const auto time = tf2_ros::fromMsg(odom->header.stamp);
+  auto base_pose_in_odom = Sophus::SE2d{};
+  tf2::convert(odom->pose.pose, base_pose_in_odom);
+  odometry_motion_buffer_.emplace_back(time, base_pose_in_odom);
+}
+
+void AmclNode::process_buffered_odometry_until(std::deque<OdometryMotion>& buffer, const tf2::TimePoint& until) {
+  while (!buffer.empty()) {
+    const auto& [odom_time, odom_pose] = buffer.front();
+    if (odom_time > until) {
+      break;
+    }
+    particle_filter_->update(odom_pose);
+    buffer.pop_front();
+  }
+}
+
 void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) {
   if (!particle_filter_) {
     RCLCPP_WARN_THROTTLE(
         get_logger(), *get_clock(), 2000, "Ignoring laser data because the particle filter has not been initialized");
     return;
   }
 
+  // If use_odometry_propagation is enabled, process odometry buffer up to lidar timestamp
+  if (get_parameter("use_odometry_propagation").as_bool()) {
+    const auto laser_scan_stamp = tf2_ros::fromMsg(laser_scan->header.stamp);
+    process_buffered_odometry_until(odometry_motion_buffer_, laser_scan_stamp);
+  }
+
+  // Get base pose in odom frame at laser scan timestamp
   auto base_pose_in_odom = Sophus::SE2d{};
   try {
     // Use the lookupTransform overload with no timeout since we're not using a dedicated

beluga_amcl/src/ros2_common.cpp

@@ -66,6 +66,12 @@ BaseAMCLNode::BaseAMCLNode(
     this->declare_parameter("scan_topic", rclcpp::ParameterValue("scan"), descriptor);
   }
 
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "Topic to subscribe to in order to receive odometry messages for motion propagation.";
+    this->declare_parameter("odom_topic", rclcpp::ParameterValue("odom"), descriptor);
+  }
+
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
     descriptor.description = "Minimum allowed number of particles.";

beluga_amcl/test/test_amcl_node.cpp

@@ -45,6 +45,15 @@ class AmclNodeUnderTest : public beluga_amcl::AmclNode {
 
   /// Return the last known estimate. Throws if there is no estimate.
   const auto& estimate() { return last_known_estimate_.value(); }
+
+  /// Check if odom_sub_ is created
+  bool has_odom_sub() const { return odom_sub_ != nullptr; }
+
+  /// Expose odometry callback for testing
+  void odometry_callback(nav_msgs::msg::Odometry::ConstSharedPtr odom) { AmclNode::odometry_callback(odom); }
+
+  /// Expose odometry_motion_buffer_ for testing
+  const auto& odometry_motion_buffer() const { return odometry_motion_buffer_; }
 };
 
 /// Base node fixture class with common utilities.
@@ -698,6 +707,156 @@ TEST_F(TestNode, TransformValue) {
   EXPECT_NEAR(transform.so2().log(), 0.0, 0.01);
 }
 
+TEST_F(TestNode, OdomSubNotCreatedWhenPropagationDisabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", false});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  // odom_sub_ should not be created when propagation is disabled
+  EXPECT_EQ(amcl_node_->has_odom_sub(), false);
+}
+
+TEST_F(TestNode, OdomSubCreatedWhenPropagationEnabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", true});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  // odom_sub_ should be created when propagation is enabled
+  EXPECT_EQ(amcl_node_->has_odom_sub(), true);
+}
+
+TEST_F(TestNode, OdometryPropagationConsumesBuffer) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", true});
+  amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  // Publish several odometry messages to fill the buffer
+  for (int i = 0; i < 5; ++i) {
+    tester_node_->publish_odometry(i * 0.1, 0.0);
+    spin_for(20ms, amcl_node_, tester_node_);
+  }
+
+  // Check that odometry_motion_buffer_ has values before laser scan
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 5);
+
+  // Configure TF so get_base_pose_in_odom works by publishing a laser scan with transform
+  tester_node_->publish_laser_scan_with_odom_to_base(Sophus::SE2d{});
+
+  // Wait for the callback to process the buffer
+  spin_for(100ms, amcl_node_, tester_node_);
+
+  // Check that odometry_motion_buffer_ was consumed up to the lidar timestamp
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 0);
+}
+
+TEST_F(TestNode, OdometryBufferConsumedWhenOdomTimestampsLessThanLidar) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", true});
+  amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  // Publish several odometry messages to fill the buffer
+  for (int i = 0; i < 5; ++i) {
+    tester_node_->publish_odometry(i * 0.1, 0.0);
+    spin_for(20ms, amcl_node_, tester_node_);
+  }
+
+  // Check that odometry_motion_buffer_ has values before laser scan
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 5);
+
+  // Configure TF so get_base_pose_in_odom works by publishing a laser scan with transform
+  tester_node_->publish_laser_scan_with_odom_to_base(Sophus::SE2d{});
+
+  // Wait for the callback to process the buffer
+  spin_for(100ms, amcl_node_, tester_node_);
+
+  // Check that odometry_motion_buffer_ was consumed up to the lidar timestamp
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 0);
+}
+
+TEST_F(TestNode, OdometryBufferConsumedWhenOdomTimestampsEqualThanLidar) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", true});
+  amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  rclcpp::Time timestamp;
+  // Publish several odometry messages to fill the buffer
+  for (int i = 0; i < 5; ++i) {
+    timestamp = tester_node_->now();
+    tester_node_->publish_odometry(i * 0.1, 0.0, 1.0, timestamp);
+    spin_for(20ms, amcl_node_, tester_node_);
+  }
+
+  // Check that odometry_motion_buffer_ has values before laser scan
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 5);
+
+  // Configure TF so get_base_pose_in_odom works by publishing a laser scan with transform
+  tester_node_->publish_laser_scan_with_odom_to_base(Sophus::SE2d{}, timestamp);
+
+  // Wait for the callback to process the buffer
+  spin_for(100ms, amcl_node_, tester_node_);
+
+  // Check that odometry_motion_buffer_ was consumed up to the lidar timestamp
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 0);
+}
+
+TEST_F(TestNode, OdometryBufferConsumedWhenOdomTimestampsMajorThanLidar) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", true});
+  amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  rclcpp::Time timestamp;
+  // Publish several odometry messages to fill the buffer
+  for (int i = 0; i < 5; ++i) {
+    timestamp = tester_node_->now();
+    tester_node_->publish_odometry(i * 0.1, 0.0, 1.0, timestamp);
+    spin_for(20ms, amcl_node_, tester_node_);
+  }
+
+  const auto newtimestamp = tester_node_->now();
+  tester_node_->publish_odometry(5 * 0.1, 0.0, 1.0, newtimestamp);
+  spin_for(20ms, amcl_node_, tester_node_);
+
+  // Check that odometry_motion_buffer_ has values before laser scan
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 6);
+
+  // Configure TF so get_base_pose_in_odom works by publishing a laser scan with transform
+  tester_node_->publish_laser_scan_with_odom_to_base(Sophus::SE2d{}, timestamp);
+
+  // Wait for the callback to process the buffer
+  spin_for(100ms, amcl_node_, tester_node_);
+
+  // Check that odometry_motion_buffer_ was consumed up to the lidar timestamp
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 1);
+}
+
+TEST_F(TestNode, OdomSubWithoutParticles) {
+  // Verify that particle filter remains uninitialized
+  EXPECT_FALSE(amcl_node_->particle_filter() != nullptr);
+
+  // Call odometry callback when particle filter is not initialized
+  const nav_msgs::msg::Odometry::SharedPtr odom = std::make_shared<nav_msgs::msg::Odometry>();
+  amcl_node_->odometry_callback(odom);
+
+  // The callback should have returned early without filling the queue
+  EXPECT_TRUE(amcl_node_->odometry_motion_buffer().empty());
+}
+
 class TestParameterValue : public ::testing::TestWithParam<rclcpp::Parameter> {};
 
 INSTANTIATE_TEST_SUITE_P(