add odometry subscriber

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

Author: fbattocchia-ekumen

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 propagation_rate 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,11 +40,11 @@
 
 #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"
 
-#include <deque>
-
 /**
  * \file
  * \brief ROS 2 integration of the 2D AMCL algorithm.
@@ -93,8 +93,8 @@ 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();
+  /// Callback for odometry updates.
+  void odometry_callback(nav_msgs::msg::Odometry::ConstSharedPtr);
 
   /// 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>;
@@ -151,8 +151,8 @@ 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_;
+  /// Odometry updates subscription.
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
 
   /// Particle filter instance.
   std::unique_ptr<beluga_ros::Amcl> particle_filter_;
@@ -162,10 +162,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};
-  /// 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_;
+  std::deque<beluga_ros::OdometryMotion> odometry_motion_buffer_;
 };
 
 }  // namespace beluga_amcl

beluga_amcl/src/amcl_node.cpp

@@ -186,12 +186,8 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", ""
 
   {
     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);
+    descriptor.description = "Set true to enable increased propagation, and set false to disable it.";
+    declare_parameter("propagation_rate", rclcpp::ParameterValue(false), descriptor);
   }
 }
 
@@ -262,15 +258,14 @@ void AmclNode::do_activate(const rclcpp_lifecycle::State&) {
       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
+    // Subscribe to odometry topic to buffer odometry motions
+    if (get_parameter("propagation_rate").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();
     }
   }
@@ -282,7 +277,7 @@ void AmclNode::do_deactivate(const rclcpp_lifecycle::State&) {
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
   global_localization_server_.reset();
-  propagation_timer_.reset();
+  odom_sub_.reset();
   if (likelihood_field_pub_) {
     likelihood_field_pub_->on_deactivate();
   }
@@ -491,38 +486,18 @@ void AmclNode::do_periodic_timer_callback() {
   }
 }
 
-auto AmclNode::get_base_pose_in_odom(const tf2::TimePoint& time) const -> std::optional<Sophus::SE2d> {
-  auto base_pose_in_odom = Sophus::SE2d{};
-  try {
-    tf2::convert(
-        tf_buffer_
-            ->lookupTransform(
-                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() {
+void AmclNode::odometry_callback(nav_msgs::msg::Odometry::ConstSharedPtr odom) {
   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()) {
+        get_logger(), *get_clock(), 2000,
+        "Ignoring odometry data because the particle filter has not been initialized");
     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());
+  // 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::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) {
@@ -533,21 +508,23 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
   }
 
   // If propagation_rate is enabled, process odometry buffer up to lidar timestamp
-  if (get_parameter("propagation_rate").as_double() > 0.0) {
+  if (get_parameter("propagation_rate").as_bool()) {
     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();
-    }
+    particle_filter_->process_buffered_odometry_until(odometry_motion_buffer_, laser_scan_stamp);
   }
 
   // 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()) {
+  auto base_pose_in_odom = Sophus::SE2d{};
+  try {
+    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))
+            .transform,
+        base_pose_in_odom);
+  } catch (const tf2::TransformException& error) {
+    RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what());
     return;
   }
 
@@ -567,7 +544,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.value(),  //
+      base_pose_in_odom,  //
       beluga_ros::LaserScan{
           laser_scan,
           laser_pose_in_base,
@@ -580,7 +557,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.value().inverse();
+    last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
     last_known_estimate_ = new_estimate;
 
     RCLCPP_INFO(

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

@@ -46,11 +46,11 @@ 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; }
+  /// Check if odom_sub_ is created
+  bool has_odom_sub() const { return odom_sub_ != nullptr; }
 
-  /// Expose propagation timer callback for testing
-  void propagation_timer_callback() { AmclNode::propagation_timer_callback(); }
+  /// 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_; }
@@ -707,44 +707,44 @@ 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});
+TEST_F(TestNode, OdomSubNotCreatedWhenPropagationDisabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", false});
   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());
+  // odom_sub_ should not be created when propagation is disabled
+  EXPECT_EQ(amcl_node_->has_odom_sub(), false);
 }
 
-TEST_F(TestNode, PropagationTimerCreatedWhenEnabled) {
-  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 10.0});
+TEST_F(TestNode, OdomSubCreatedWhenPropagationEnabled) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", true});
   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());
+  // odom_sub_ should be created when propagation is enabled
+  EXPECT_EQ(amcl_node_->has_odom_sub(), true);
 }
 
-TEST_F(TestNode, PropagationTimerIntegrationTest) {
-  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 5.0});
+TEST_F(TestNode, PropagationRate) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"propagation_rate", 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());
 
-  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_);
+  // 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_FALSE(amcl_node_->odometry_motion_buffer().empty());
-  const auto buffer_size_before = amcl_node_->odometry_motion_buffer().size();
+  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{});
@@ -753,38 +753,20 @@ TEST_F(TestNode, PropagationTimerIntegrationTest) {
   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);
+  EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 0);
 
   // 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) {
+TEST_F(TestNode, OdomSubWithoutParticles) {
   // 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();
+  // 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());