From b99b7fc5b024d04d03bc9465bdaffe0c03014fc9 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 15:48:36 -0700 Subject: [PATCH 1/6] Update amcl_node.hpp Signed-off-by: Steve Macenski --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index c7341a5985a..8c4a2b85560 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -171,8 +171,7 @@ class AmclNode : public nav2_util::LifecycleNode * @brief Initialize incoming data message subscribers and filters */ void initMessageFilters(); - std::unique_ptr> laser_scan_sub_; + std::unique_ptr> laser_scan_sub_; std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; From bd2e08b3fbd4336a660c39a54a28954027cc9565 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 15:49:15 -0700 Subject: [PATCH 2/6] Update amcl_node.cpp Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 71b50bdcc79..f28ed78b000 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1523,9 +1523,8 @@ AmclNode::initMessageFilters() { auto sub_opt = rclcpp::SubscriptionOptions(); sub_opt.callback_group = callback_group_; - laser_scan_sub_ = std::make_unique>( - shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); laser_scan_filter_ = std::make_unique>( *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, From dfafef18f9f1667ab8fd6c66489bb473033e0f6b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 18:38:15 -0700 Subject: [PATCH 3/6] Working for Kilted, Jazzy Signed-off-by: Steve Macenski --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 8 ++++++ nav2_amcl/src/amcl_node.cpp | 10 +++++++ .../nav2_costmap_2d/obstacle_layer.hpp | 6 ++++ nav2_costmap_2d/plugins/obstacle_layer.cpp | 28 ++++++++++++++++--- 4 files changed, 48 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 8c4a2b85560..58b48246a9d 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -29,6 +29,7 @@ #include #include "message_filters/subscriber.hpp" +#include "rclcpp/version.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -171,7 +172,14 @@ class AmclNode : public nav2_util::LifecycleNode * @brief Initialize incoming data message subscribers and filters */ void initMessageFilters(); + + // To Support Kilted and Older from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) std::unique_ptr> laser_scan_sub_; + #else + std::unique_ptr> laser_scan_sub_; + #endif std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index f28ed78b000..da92c6b5ab4 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1523,6 +1523,16 @@ AmclNode::initMessageFilters() { auto sub_opt = rclcpp::SubscriptionOptions(); sub_opt.callback_group = callback_group_; + + #if RCLCPP_VERSION_GTE(29, 6, 0) + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); + #else + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); + #endif + laser_scan_sub_ = std::make_unique>( shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 1cdec808da7..16113d61b16 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -43,6 +43,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rclcpp/version.h" #include "laser_geometry/laser_geometry.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wreorder" @@ -234,8 +235,13 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to project laser scans into point clouds laser_geometry::LaserProjection projector_; /// @brief Used for the observation message filters + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::vector> + observation_subscribers_; + #else std::vector>> observation_subscribers_; + #endif /// @brief Used to make sure that transforms are available for each sensor std::vector> observation_notifiers_; /// @brief Used to store observations from various sensors diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index c1717507bec..1102fd09a1c 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -231,13 +231,23 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { + // For Kilted and Older Support from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::shared_ptr> sub; + #else std::shared_ptr> sub; + #endif - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) + // For Kilted compatibility in Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + sub = std::make_shared>( + node, topic, custom_qos_profile, sub_opt); + // For Jazzy compatibility in Message Filters API change + #elif RCLCPP_VERSION_GTE(29, 0, 0) sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + // For Humble and Older compatibility in Message Filters API change #else sub = std::make_shared>( @@ -271,13 +281,23 @@ void ObstacleLayer::onInitialize() observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { + // For Kilted and Older Support from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::shared_ptr> sub; + #else std::shared_ptr> sub; + #endif - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) + // For Kilted compatibility in Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + sub = std::make_shared>( + node, topic, custom_qos_profile, sub_opt); + // For Jazzy compatibility in Message Filters API change + #elif RCLCPP_VERSION_GTE(29, 0, 0) sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + // For Humble and Older compatibility in Message Filters API change #else sub = std::make_shared>( From 9345ed3467ac22bd7a58c9c36a255586f7dd8ff1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 18:54:37 -0700 Subject: [PATCH 4/6] Update amcl_node.cpp Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index da92c6b5ab4..c3296d4d23d 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1529,7 +1529,7 @@ AmclNode::initMessageFilters() shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); #else laser_scan_sub_ = std::make_unique>( + rclcpp_lifecycle::LifecycleNode>>( shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); #endif From a78003104f2307c43d41b0e4e8472b929fac39d2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 18:55:03 -0700 Subject: [PATCH 5/6] Update amcl_node.cpp Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c3296d4d23d..dce3e83d611 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1529,7 +1529,7 @@ AmclNode::initMessageFilters() shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); #else laser_scan_sub_ = std::make_unique>( + rclcpp_lifecycle::LifecycleNode>>( shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); #endif From 20856b826d81fda47c65ebfd8bad740e7ef2fcba Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 19:18:38 -0700 Subject: [PATCH 6/6] Update amcl_node.cpp Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index dce3e83d611..953e5e05509 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1533,9 +1533,6 @@ AmclNode::initMessageFilters() shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); #endif - laser_scan_sub_ = std::make_unique>( - shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); - laser_scan_filter_ = std::make_unique>( *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, get_node_logging_interface(),