Add timer-based propagation functionality to AMCL

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -43,6 +43,8 @@
 #include "beluga_amcl/message_filters.hpp"
 #include "beluga_amcl/ros2_common.hpp"
 
+#include <deque>
+
 /**
  * \file
  * \brief ROS 2 integration of the 2D AMCL algorithm.
@@ -91,6 +93,12 @@ class AmclNode : public BaseAMCLNode {
   /// Callback for laser scan updates.
   void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr);
 
+  /// Callback for increased propagation timer.
+  void propagation_timer_callback();
+
+  /// Helper function to get base pose in odom frame at specific time.
+  auto get_base_pose_in_odom(const tf2::TimePoint& time) const -> std::optional<Sophus::SE2d>;
+
   /// Callback for pose (re)initialization.
   void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override;
 
@@ -143,6 +151,9 @@ class AmclNode : public BaseAMCLNode {
   /// Connection for laser scan updates filter and callback.
   ::message_filters::Connection laser_scan_connection_;
 
+  /// Timer for increased propagation rate.
+  rclcpp::TimerBase::SharedPtr propagation_timer_;
+
   /// Particle filter instance.
   std::unique_ptr<beluga_ros::Amcl> particle_filter_;
   /// Last known pose estimate, if any.
@@ -151,6 +162,10 @@ 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};
+  /// Type Buffer for queued odometry motions (timestamp, pose)
+  using OdometryMotion = std::pair<tf2::TimePoint, Sophus::SE2d>;
+  /// 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,16 @@ 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 = "Frequency in Hz for increased propagation rate. Set to 0 to disable.";
+    descriptor.floating_point_range.resize(1);
+    descriptor.floating_point_range[0].from_value = 0.0;
+    descriptor.floating_point_range[0].to_value = 100.0;
+    descriptor.floating_point_range[0].step = 0.0;
+    declare_parameter("propagation_rate", rclcpp::ParameterValue(0.0), descriptor);
+  }
 }
 
 AmclNode::~AmclNode() {
@@ -251,6 +261,19 @@ 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");
+
+  // Setup increased propagation timer if enabled
+  {
+    const double propagation_freq = get_parameter("propagation_rate").as_double();
+    if (propagation_freq > 0.0) {
+      auto period = std::chrono::duration<double>(1.0 / propagation_freq);
+      propagation_timer_ =
+          create_wall_timer(period, std::bind(&AmclNode::propagation_timer_callback, this), common_callback_group_);
+      RCLCPP_INFO(get_logger(), "Created propagation timer at %.1f Hz", propagation_freq);
+      // Initialize odometry motion buffer
+      odometry_motion_buffer_.clear();
+    }
+  }
 }
 
 void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) {
@@ -259,6 +282,7 @@ void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) {
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
   global_localization_server_.reset();
+  propagation_timer_.reset();
   if (likelihood_field_pub_) {
     likelihood_field_pub_->on_deactivate();
   }
@@ -268,6 +292,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,26 +491,63 @@ void AmclNode::do_periodic_timer_callback() {
   }
 }
 
-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;
-  }
-
+auto AmclNode::get_base_pose_in_odom(const tf2::TimePoint& time) const -> std::optional<Sophus::SE2d> {
   auto base_pose_in_odom = Sophus::SE2d{};
   try {
-    // Use the lookupTransform overload with no timeout since we're not using a dedicated
-    // tf thread. The message filter we are using avoids the need for it.
     tf2::convert(
         tf_buffer_
             ->lookupTransform(
-                get_parameter("odom_frame_id").as_string(), get_parameter("base_frame_id").as_string(),
-                tf2_ros::fromMsg(laser_scan->header.stamp))
+                get_parameter("odom_frame_id").as_string(), get_parameter("base_frame_id").as_string(), time)
             .transform,
         base_pose_in_odom);
+    return base_pose_in_odom;
   } catch (const tf2::TransformException& error) {
     RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what());
+    return std::nullopt;
+  }
+}
+
+void AmclNode::propagation_timer_callback() {
+  if (!particle_filter_) {
+    RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 2000, "Ignoring propagation because the particle filter has not been initialized");
+    return;
+  }
+
+  const auto base_pose_in_odom = get_base_pose_in_odom(tf2::TimePointZero);
+  if (!base_pose_in_odom.has_value()) {
+    return;
+  }
+
+  const auto now = this->now();
+  const auto time = tf2_ros::fromMsg(now);
+  // Queue odometry motion (timestamp, pose) for later processing in laser_callback
+  odometry_motion_buffer_.emplace_back(time, base_pose_in_odom.value());
+}
+
+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 propagation_rate is enabled, process odometry buffer up to lidar timestamp
+  if (get_parameter("propagation_rate").as_double() > 0.0) {
+    const auto laser_scan_stamp = tf2_ros::fromMsg(laser_scan->header.stamp);
+    while (!odometry_motion_buffer_.empty()) {
+      const auto& [odom_time, odom_pose] = odometry_motion_buffer_.front();
+      if (odom_time >= laser_scan_stamp) {
+        break;
+      }
+      particle_filter_->update(odom_pose);
+      odometry_motion_buffer_.pop_front();
+    }
+  }
+
+  // Get base pose in odom frame at laser scan timestamp
+  const auto base_pose_in_odom = get_base_pose_in_odom(tf2_ros::fromMsg(laser_scan->header.stamp));
+  if (!base_pose_in_odom.has_value()) {
     return;
   }
 
@@ -505,7 +567,7 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
 
   const auto update_start_time = std::chrono::high_resolution_clock::now();
   const auto new_estimate = particle_filter_->update(
-      base_pose_in_odom,  //
+      base_pose_in_odom.value(),  //
       beluga_ros::LaserScan{
           laser_scan,
           laser_pose_in_base,
@@ -518,7 +580,7 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
 
   if (new_estimate.has_value()) {
     const auto& [base_pose_in_map, _] = new_estimate.value();
-    last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
+    last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.value().inverse();
     last_known_estimate_ = new_estimate;
 
     RCLCPP_INFO(

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 propagation timer is created
+  bool has_propagation_timer() const { return propagation_timer_ != nullptr; }
+
+  /// Expose propagation timer callback for testing
+  void propagation_timer_callback() { AmclNode::propagation_timer_callback(); }
+
+  /// 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,89 @@ TEST_F(TestNode, TransformValue) {
   EXPECT_NEAR(transform.so2().log(), 0.0, 0.01);
 }
 
+TEST_F(TestNode, PropagationTimerNotCreatedWhenDisabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 0.0});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  // Timer should not be created when frequency is 0
+  EXPECT_FALSE(amcl_node_->has_propagation_timer());
+}
+
+TEST_F(TestNode, PropagationTimerCreatedWhenEnabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 10.0});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+
+  // Timer should be created when frequency > 0
+  EXPECT_TRUE(amcl_node_->has_propagation_timer());
+}
+
+TEST_F(TestNode, PropagationTimerIntegrationTest) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 5.0});
+  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());
+
+  tester_node_->publish_odom_to_base_tf(Sophus::SE2d{});
+
+  // Wait for several timer executions to fill the odometry buffer
+  spin_for(500ms, amcl_node_, tester_node_);
+
+  // Check that odometry_motion_buffer_ has values before laser scan
+  EXPECT_FALSE(amcl_node_->odometry_motion_buffer().empty());
+  const auto buffer_size_before = amcl_node_->odometry_motion_buffer().size();
+
+  // 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_LT(amcl_node_->odometry_motion_buffer().size(), buffer_size_before);
+
+  // Verify particle filter still exists and has particles
+  EXPECT_TRUE(amcl_node_->particle_filter() != nullptr);
+  EXPECT_GT(amcl_node_->particle_filter()->particles().size(), 0UL);
+}
+
+TEST_F(TestNode, PropagationTimerWithoutParticles) {
+  // Verify that particle filter remains uninitialized
+  EXPECT_FALSE(amcl_node_->particle_filter() != nullptr);
+
+  // Call propagation timer callback when particle filter is not initialized
+  amcl_node_->propagation_timer_callback();
+
+  // The callback should have returned early without filling the queue
+  EXPECT_TRUE(amcl_node_->odometry_motion_buffer().empty());
+}
+
+TEST_F(TestNode, PropagationTimerWithoutBaseToOdom) {
+  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());
+
+  // Particle filter is initialized but no odom->base transform is available
+  EXPECT_TRUE(amcl_node_->is_initialized());
+  EXPECT_TRUE(amcl_node_->particle_filter() != nullptr);
+
+  // Call propagation callback without any transform data
+  // This should return early because get_base_pose_in_odom() will fail
+  amcl_node_->propagation_timer_callback();
+
+  // 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(

beluga_amcl/test/test_utils/node_testing.hpp

@@ -244,6 +244,15 @@ class TesterNode : public rclcpp::Node {
     tf_broadcaster_->sendTransform(transform_laser);
   }
 
+  void publish_odom_to_base_tf(const Sophus::SE2d& transform) {
+    auto msg = geometry_msgs::msg::TransformStamped{};
+    msg.header.stamp = now();
+    msg.header.frame_id = "odom";
+    msg.child_frame_id = "base_footprint";
+    msg.transform = tf2::toMsg(transform);
+    tf_broadcaster_->sendTransform(msg);
+  }
+
   void publish_laser_scan_with_odom_to_base(const Sophus::SE2d& transform) {
     const auto timestamp = now();
 

beluga_ros/include/beluga_ros/amcl.hpp

@@ -210,6 +210,16 @@ class Amcl {
   /// Update the map used for localization.
   void update_map(beluga_ros::OccupancyGrid map);
 
+  /// Update particles based on motion only (propagation only).
+  /**
+   * This method only performs the propagation step of the particle filter update,
+   * applying the motion model without any sensor correction. Useful for forced
+   * propagation at regular intervals without waiting for sensor data.
+   *
+   * \param base_pose_in_odom Base pose in the odometry frame.
+   */
+  void update(Sophus::SE2d base_pose_in_odom);
+
   /// Update particles based on motion and sensor information.
   /**
    * This method performs a particle filter update step using motion and sensor data. It evaluates whether

beluga_ros/src/amcl.cpp

@@ -50,6 +50,16 @@ void Amcl::update_map(beluga_ros::OccupancyGrid map) {
   std::visit([&](auto& sensor_model) { sensor_model.update_map(std::move(map)); }, sensor_model_);
 }
 
+void Amcl::update(Sophus::SE2d base_pose_in_odom) {
+  if (!particles_.empty()) {
+    std::visit(
+        [&, this](auto& policy, auto& motion_model) {
+          particles_ |= beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom));
+        },
+        execution_policy_, motion_model_);
+  }
+}
+
 auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_scan)
     -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>> {
   if (particles_.empty()) {

beluga_ros/test/test_amcl.cpp

@@ -148,5 +148,4 @@ TEST(TestAmcl, UpdateWithParticlesForced) {
   estimate = amcl.update(Sophus::SE2d{}, make_dummy_laser_scan());
   ASSERT_TRUE(estimate.has_value());
 }
-
 }  // namespace