diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index c7341a5985a..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,8 +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 71b50bdcc79..953e5e05509 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1523,9 +1523,15 @@ 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_filter_ = std::make_unique>( *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, 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>(