diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index e67c8ea7719..040e3f51a37 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) find_package(nav2_ros_common REQUIRED) +find_package(point_cloud_transport REQUIRED) nav2_package() @@ -54,6 +55,7 @@ target_link_libraries(${monitor_library_name} PUBLIC tf2::tf2 tf2_ros::tf2_ros ${visualization_msgs_TARGETS} + point_cloud_transport::point_cloud_transport ) target_link_libraries(${monitor_library_name} PRIVATE rclcpp_components::component @@ -88,6 +90,7 @@ target_link_libraries(${detector_library_name} PUBLIC tf2_ros::tf2_ros tf2::tf2 ${visualization_msgs_TARGETS} + point_cloud_transport::point_cloud_transport ) target_link_libraries(${detector_library_name} PRIVATE rclcpp_components::component @@ -169,6 +172,7 @@ ament_export_dependencies( tf2_ros nav2_ros_common visualization_msgs + point_cloud_transport ) ament_export_targets(export_${PROJECT_NAME}) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 8a46d2b65c5..c94180f94cf 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -20,6 +20,7 @@ #include #include "sensor_msgs/msg/point_cloud2.hpp" +#include "point_cloud_transport/point_cloud_transport.hpp" #include "nav2_collision_monitor/source.hpp" @@ -98,7 +99,15 @@ class PointCloud : public Source // ----- Variables ----- /// @brief PointCloud data subscriber + #if RCLCPP_VERSION_GTE(30, 0, 0) + std::shared_ptr pct_; + point_cloud_transport::Subscriber data_sub_; + #else nav2::Subscription::SharedPtr data_sub_; + #endif + + // Transport type used for PointCloud messages (e.g., raw or compressed) + std::string transport_type_; // Minimum and maximum height of PointCloud projected to 2D space double min_height_, max_height_; diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 6a8e5ace75b..6b009f1634b 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -25,6 +25,8 @@ tf2_ros visualization_msgs nav2_ros_common + point_cloud_transport + point_cloud_transport_plugins ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index eb3ee0a8739..bbeaadc5154 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -23,6 +23,7 @@ collision_detector: pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" + transport_type: "raw" # Options: raw, zlib, draco, zstd min_height: 0.1 max_height: 0.5 enabled: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index eac7486913c..28224362ea9 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -96,6 +96,7 @@ collision_monitor: pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" + transport_type: "raw" # Options: raw, zlib, draco, zstd min_height: 0.1 max_height: 0.5 enabled: True diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 590eacb3cb8..72ef467b9a5 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -45,7 +45,11 @@ PointCloud::PointCloud( PointCloud::~PointCloud() { RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str()); + #if RCLCPP_VERSION_GTE(30, 0, 0) + data_sub_.shutdown(); + #else data_sub_.reset(); + #endif } void PointCloud::configure() @@ -60,10 +64,20 @@ void PointCloud::configure() getParameters(source_topic); + #if RCLCPP_VERSION_GTE(30, 0, 0) + const point_cloud_transport::TransportHints hint(transport_type_); + pct_ = std::make_shared(*node); + data_sub_ = pct_->subscribe( + source_topic, nav2::qos::SensorDataQoS(), + std::bind(&PointCloud::dataCallback, this, std::placeholders::_1), + {}, &hint + ); + #else data_sub_ = node->create_subscription( source_topic, std::bind(&PointCloud::dataCallback, this, std::placeholders::_1), nav2::qos::SensorDataQoS()); + #endif // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -131,6 +145,9 @@ void PointCloud::getParameters(std::string & source_topic) nav2::declare_parameter_if_not_declared( node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0)); min_range_ = node->get_parameter(source_name_ + ".min_range").as_double(); + nav2::declare_parameter_if_not_declared( + node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw"))); + transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string(); } void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 851f2428b42..d7d74038949 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED) find_package(nav2_ros_common REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(point_cloud_transport REQUIRED) nav2_package() @@ -88,6 +89,7 @@ target_link_libraries(layers PUBLIC nav2_ros_common::nav2_ros_common tf2::tf2 nav2_ros_common::nav2_ros_common + point_cloud_transport::point_cloud_transport ) target_link_libraries(layers PRIVATE pluginlib::pluginlib @@ -222,6 +224,7 @@ ament_export_dependencies( tf2_ros tf2_sensor_msgs nav2_ros_common + point_cloud_transport ) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() 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 5e66e24795a..32b17ca5d5b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -51,6 +51,7 @@ #pragma GCC diagnostic pop #include "message_filters/subscriber.hpp" +#include "point_cloud_transport/subscriber_filter.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "sensor_msgs/msg/laser_scan.hpp" diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index c69d467b908..4e4940a59af 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -42,6 +42,8 @@ tf2_sensor_msgs visualization_msgs nav2_ros_common + point_cloud_transport + point_cloud_transport_plugins ament_lint_common ament_lint_auto diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index bac51b92f92..b897ac515be 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -141,7 +141,7 @@ void ObstacleLayer::onInitialize() while (ss >> source) { // get the parameters for the specific topic double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; - std::string topic, sensor_frame, data_type; + std::string topic, sensor_frame, data_type, transport_type; bool inf_is_valid, clearing, marking; declareParameter(source + "." + "topic", rclcpp::ParameterValue(source)); @@ -158,6 +158,7 @@ void ObstacleLayer::onInitialize() declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0)); declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0)); declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0)); + declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw"))); node->get_parameter(name_ + "." + source + "." + "topic", topic); node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); @@ -173,6 +174,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); node->get_parameter(name_ + "." + source + "." + "marking", marking); node->get_parameter(name_ + "." + source + "." + "clearing", clearing); + node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type); if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { RCLCPP_FATAL( @@ -287,16 +289,23 @@ void ObstacleLayer::onInitialize() tf_filter_tolerance)); } else { + // For Rolling and Newer Support from PointCloudTransport API change + #if RCLCPP_VERSION_GTE(30, 0, 0) + std::shared_ptr sub; // For Kilted and Older Support from Message Filters API change - #if RCLCPP_VERSION_GTE(29, 6, 0) + #elif RCLCPP_VERSION_GTE(29, 6, 0) std::shared_ptr> sub; #else std::shared_ptr> sub; #endif + // For Rolling compatibility in PointCloudTransport API change + #if RCLCPP_VERSION_GTE(30, 0, 0) + sub = std::make_shared( + *node, topic, transport_type, custom_qos_profile, sub_opt); // For Kilted compatibility in Message Filters API change - #if RCLCPP_VERSION_GTE(29, 6, 0) + #elif 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