added updates lazily of propagation model

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

Author: fbattocchia-ekumen

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.
@@ -160,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};
+
+  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

@@ -191,7 +191,7 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", ""
     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);
+    declare_parameter("propagation_rate", rclcpp::ParameterValue(0.0), descriptor);
   }
 }
 
@@ -264,12 +264,14 @@ void AmclNode::do_activate(const rclcpp_lifecycle::State&) {
 
   // Setup increased propagation timer if enabled
   {
-    const double propagation_freq = get_parameter("increase_propagation").as_double();
+    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();
     }
   }
 }
@@ -290,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>> {
@@ -499,7 +502,7 @@ auto AmclNode::get_base_pose_in_odom(const tf2::TimePoint& time) const -> std::o
         base_pose_in_odom);
     return base_pose_in_odom;
   } catch (const tf2::TransformException& error) {
-    RCLCPP_WARN(get_logger(), "Could not get base pose in odom: %s", error.what());
+    RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what());
     return std::nullopt;
   }
 }
@@ -511,16 +514,15 @@ void AmclNode::propagation_timer_callback() {
     return;
   }
 
-  // Get current base pose in odom frame (latest available)
-  auto base_pose_in_odom = get_base_pose_in_odom(tf2::TimePointZero);
+  const 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");
+  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) {
@@ -530,8 +532,21 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
     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
-  auto base_pose_in_odom = get_base_pose_in_odom(tf2_ros::fromMsg(laser_scan->header.stamp));
+  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;
   }

beluga_amcl/test/test_amcl_node.cpp

@@ -51,6 +51,9 @@ class AmclNodeUnderTest : public beluga_amcl::AmclNode {
 
   /// 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.
@@ -705,7 +708,7 @@ TEST_F(TestNode, TransformValue) {
 }
 
 TEST_F(TestNode, PropagationTimerNotCreatedWhenDisabled) {
-  amcl_node_->set_parameter(rclcpp::Parameter{"increase_propagation", 0.0});
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 0.0});
   amcl_node_->configure();
   amcl_node_->activate();
   tester_node_->publish_map();
@@ -716,7 +719,7 @@ TEST_F(TestNode, PropagationTimerNotCreatedWhenDisabled) {
 }
 
 TEST_F(TestNode, PropagationTimerCreatedWhenEnabled) {
-  amcl_node_->set_parameter(rclcpp::Parameter{"increase_propagation", 10.0});
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 10.0});
   amcl_node_->configure();
   amcl_node_->activate();
   tester_node_->publish_map();
@@ -727,32 +730,45 @@ TEST_F(TestNode, PropagationTimerCreatedWhenEnabled) {
 }
 
 TEST_F(TestNode, PropagationTimerIntegrationTest) {
-  amcl_node_->set_parameter(rclcpp::Parameter{"increase_propagation", 5.0});
+  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 several timer executions
-  spin_for(500ms, amcl_node_, tester_node_);
+  // 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
-  // This should trigger early return due to !particle_filter_ check
   amcl_node_->propagation_timer_callback();
 
-  // Verify that particle filter remains uninitialized
-  // The callback should have returned early without creating the filter
-  EXPECT_FALSE(amcl_node_->particle_filter() != nullptr);
+  // The callback should have returned early without filling the queue
+  EXPECT_TRUE(amcl_node_->odometry_motion_buffer().empty());
 }
 
 TEST_F(TestNode, PropagationTimerWithoutBaseToOdom) {
@@ -766,17 +782,12 @@ TEST_F(TestNode, PropagationTimerWithoutBaseToOdom) {
   EXPECT_TRUE(amcl_node_->is_initialized());
   EXPECT_TRUE(amcl_node_->particle_filter() != nullptr);
 
-  // Capture the current particles before calling the propagation callback
-  const auto particles_before = amcl_node_->particle_filter()->particles();
-  EXPECT_GT(particles_before.size(), 0UL);
-
   // Call propagation callback without any transform data
   // This should return early because get_base_pose_in_odom() will fail
   amcl_node_->propagation_timer_callback();
 
-  // Verify particles remain unchanged since no propagation occurred
-  const auto particles_after = amcl_node_->particle_filter()->particles();
-  EXPECT_EQ(particles_before.size(), particles_after.size());
+  // 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> {};

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

@@ -218,7 +218,7 @@ class Amcl {
    *
    * \param base_pose_in_odom Base pose in the odometry frame.
    */
-  void update_propagation(Sophus::SE2d base_pose_in_odom);
+  void update(Sophus::SE2d base_pose_in_odom);
 
   /// Update particles based on motion and sensor information.
   /**

beluga_ros/src/amcl.cpp

@@ -50,18 +50,14 @@ 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;
+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_);
   }
-
-  // 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)

beluga_ros/test/test_amcl.cpp

@@ -148,11 +148,4 @@ TEST(TestAmcl, UpdateWithParticlesForced) {
   estimate = amcl.update(Sophus::SE2d{}, make_dummy_laser_scan());
   ASSERT_TRUE(estimate.has_value());
 }
-
-TEST(TestAmcl, UpdatePropagationWithNoParticles) {
-  auto amcl = make_amcl();
-  amcl.update_propagation(Sophus::SE2d{});
-  ASSERT_EQ(amcl.particles().size(), 0);
-}
-
 }  // namespace