feat: Add timer-based propagation functionality to AMCL

Author: fbattocchia-ekumen

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -91,6 +91,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 +149,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.

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("increase_propagation", rclcpp::ParameterValue(0.0), descriptor);
+  }
 }
 
 AmclNode::~AmclNode() {
@@ -251,6 +261,17 @@ 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("increase_propagation").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);
+    }
+  }
 }
 
 void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) {
@@ -259,6 +280,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();
   }
@@ -466,26 +488,51 @@ 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());
+    RCLCPP_WARN(get_logger(), "Could not get base pose in odom: %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;
+  }
+
+  // Get current base pose in odom frame (latest available)
+  auto base_pose_in_odom = get_base_pose_in_odom(tf2::TimePointZero);
+  if (!base_pose_in_odom.has_value()) {
+    return;
+  }
+
+  // Force a propagation-only update (without sensor data)
+  particle_filter_->update_propagation(base_pose_in_odom.value());
+
+  RCLCPP_INFO(get_logger(), "Forced propagation update executed");
+}
+
+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;
+  }
+
+  // Get base pose in odom frame at laser scan timestamp
+  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 +552,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 +565,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,9 @@ 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; }
 };
 
 /// Base node fixture class with common utilities.
@@ -698,6 +701,47 @@ TEST_F(TestNode, TransformValue) {
   EXPECT_NEAR(transform.so2().log(), 0.0, 0.01);
 }
 
+TEST_F(TestNode, PropagationTimerNotCreatedWhenDisabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"increase_propagation", 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{"increase_propagation", 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{"increase_propagation", 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());
+
+  // 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 several timer executions
+  spin_for(500ms, amcl_node_, tester_node_);
+
+  // Verify particle filter still exists and has particles
+  EXPECT_TRUE(amcl_node_->particle_filter() != nullptr);
+  EXPECT_GT(amcl_node_->particle_filter()->particles().size(), 0UL);
+}
+
 class TestParameterValue : public ::testing::TestWithParam<rclcpp::Parameter> {};
 
 INSTANTIATE_TEST_SUITE_P(

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_propagation(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,20 @@ 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_propagation(Sophus::SE2d base_pose_in_odom) {
+  if (particles_.empty()) {
+    return;
+  }
+
+  // Force propagation without checking update_policy_
+  std::visit(
+      [&, this](auto& policy, auto& motion_model) {
+        particles_ |= beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom)) |
+                      beluga::actions::normalize(policy);
+      },
+      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()) {