diff --git a/beluga_amcl/README.md b/beluga_amcl/README.md index 7e860e1ca..1dabd2751 100644 --- a/beluga_amcl/README.md +++ b/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 diff --git a/beluga_amcl/config/Amcl.cfg b/beluga_amcl/config/Amcl.cfg index c4c4c36cd..34b114cbc 100755 --- a/beluga_amcl/config/Amcl.cfg +++ b/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.", diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 67db153d9..615acf264 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "beluga_amcl/message_filters.hpp" #include "beluga_amcl/ros2_common.hpp" @@ -60,6 +61,9 @@ class AmclNode : public BaseAMCLNode { ~AmclNode() override; protected: + /// Type Buffer for queued odometry motions (timestamp, pose) + using OdometryMotion = std::pair; + /// Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state. void do_activate(const rclcpp_lifecycle::State&) override; @@ -91,6 +95,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(const tf2::TimePoint& until); + /// Callback for pose (re)initialization. void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override; @@ -142,7 +152,6 @@ class AmclNode : public BaseAMCLNode { std::unique_ptr> laser_scan_filter_; /// Connection for laser scan updates filter and callback. ::message_filters::Connection laser_scan_connection_; - /// Particle filter instance. std::unique_ptr particle_filter_; /// Last known pose estimate, if any. @@ -151,6 +160,10 @@ class AmclNode : public BaseAMCLNode { std::optional 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 odometry_motion_buffer_; + /// Odometry updates subscription. + rclcpp::Subscription::SharedPtr odom_sub_; }; } // namespace beluga_amcl diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index fc74e8b71..5e3d9012f 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/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 = "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( + 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 odometry 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> { @@ -466,6 +486,31 @@ 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(const tf2::TimePoint& until) { + while (!odometry_motion_buffer_.empty()) { + const auto& [odom_time, odom_pose] = odometry_motion_buffer_.front(); + if (odom_time > until) { + break; + } + particle_filter_->update(odom_pose); + odometry_motion_buffer_.pop_front(); + } +} + void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { if (!particle_filter_) { RCLCPP_WARN_THROTTLE( @@ -473,6 +518,11 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ return; } + // If use_odometry_propagation is enabled, process odometry buffer up to lidar timestamp + const auto laser_scan_stamp = tf2_ros::fromMsg(laser_scan->header.stamp); + process_buffered_odometry_until(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 diff --git a/beluga_amcl/src/ros2_common.cpp b/beluga_amcl/src/ros2_common.cpp index 92882e6ce..a5cc70bd3 100644 --- a/beluga_amcl/src/ros2_common.cpp +++ b/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."; diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp index d6b67a0d1..e07e145a7 100644 --- a/beluga_amcl/test/test_amcl_node.cpp +++ b/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,155 @@ TEST_F(TestNode, TransformValue) { EXPECT_NEAR(transform.so2().log(), 0.0, 0.01); } +TEST_F(TestNode, OdomSubNotCreatedWhenPropagationDisabled) { + 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, OdometryBufferConsumedWhenOdomTimestampsGreaterThanLidar) { + 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(); + 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 {}; INSTANTIATE_TEST_SUITE_P( diff --git a/beluga_amcl/test/test_utils/node_testing.hpp b/beluga_amcl/test/test_utils/node_testing.hpp index b7d24c567..6f8fd8984 100644 --- a/beluga_amcl/test/test_utils/node_testing.hpp +++ b/beluga_amcl/test/test_utils/node_testing.hpp @@ -28,17 +28,17 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include -#include - namespace beluga_amcl::testing { using namespace std::chrono_literals; @@ -54,6 +54,8 @@ class TesterNode : public rclcpp::Node { laser_scan_publisher_ = create_publisher("scan", rclcpp::SystemDefaultsQoS()); + odometry_publisher_ = create_publisher("odom", rclcpp::SensorDataQoS()); + point_cloud_publisher_ = create_publisher("point_cloud", rclcpp::SystemDefaultsQoS()); @@ -244,8 +246,10 @@ class TesterNode : public rclcpp::Node { tf_broadcaster_->sendTransform(transform_laser); } - void publish_laser_scan_with_odom_to_base(const Sophus::SE2d& transform) { - const auto timestamp = now(); + void publish_laser_scan_with_odom_to_base( + const Sophus::SE2d& transform, + std::optional stamp = std::nullopt) { + const auto timestamp = stamp.value_or(now()); auto scan = sensor_msgs::msg::LaserScan{}; scan.header.stamp = timestamp; @@ -267,6 +271,18 @@ class TesterNode : public rclcpp::Node { tf_broadcaster_->sendTransform(transform_laser); } + void + publish_odometry(double x, double y, double orientation_w = 1.0, std::optional stamp = std::nullopt) { + auto odom_msg = nav_msgs::msg::Odometry{}; + odom_msg.header.stamp = stamp.value_or(now()); + odom_msg.header.frame_id = "odom"; + odom_msg.child_frame_id = "base_footprint"; + odom_msg.pose.pose.position.x = x; + odom_msg.pose.pose.position.y = y; + odom_msg.pose.pose.orientation.w = orientation_w; + odometry_publisher_->publish(odom_msg); + } + void publish_3d_laser_scan_with_odom_to_base(const Sophus::SE3d& transform) { const auto timestamp = now(); @@ -344,6 +360,7 @@ class TesterNode : public rclcpp::Node { PublisherPtr map_publisher_; PublisherPtr initial_pose_publisher_; PublisherPtr laser_scan_publisher_; + PublisherPtr odometry_publisher_; PublisherPtr point_cloud_publisher_; template diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index f2e3d88f7..ff36df64d 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/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 diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index ead2b4f5f..f92b7d4fb 100644 --- a/beluga_ros/src/amcl.cpp +++ b/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> { if (particles_.empty()) {