Include option to use PointCloud Transport (#5264)

* Rebase

Signed-off-by: ElSayed ElSheikh <[email protected]>

* Make linters happy

Signed-off-by: ElSayed ElSheikh <[email protected]>

* Rename point_cloud_transport parameter

Signed-off-by: ElSayed ElSheikh <[email protected]>

* Rebase

Signed-off-by: ElSayed ElSheikh <[email protected]>

* Feedback

Signed-off-by: ElSayed ElSheikh <[email protected]>

* Feedback

Signed-off-by: ElSayed ElSheikh <[email protected]>

* Fix

Signed-off-by: ElSayed ElSheikh <[email protected]>

---------

Signed-off-by: ElSayed ElSheikh <[email protected]>

Author: elsayedelsheikh

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})
 

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -20,6 +20,7 @@
 #include <string>
 
 #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<point_cloud_transport::PointCloudTransport> pct_;
+  point_cloud_transport::Subscriber data_sub_;
+  #else
   nav2::Subscription<sensor_msgs::msg::PointCloud2>::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_;

nav2_collision_monitor/package.xml

@@ -25,6 +25,8 @@
   <depend>tf2_ros</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>

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

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

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<point_cloud_transport::PointCloudTransport>(*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<sensor_msgs::msg::PointCloud2>(
     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)

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()

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"

nav2_costmap_2d/package.xml

@@ -42,6 +42,8 @@
   <depend>tf2_sensor_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

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<point_cloud_transport::SubscriberFilter> 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<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> sub;
       #else
       std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
         rclcpp_lifecycle::LifecycleNode>> sub;
       #endif
 
+      // For Rolling compatibility in PointCloudTransport API change
+      #if RCLCPP_VERSION_GTE(30, 0, 0)
+      sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
+        *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<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(
         node, topic, custom_qos_profile, sub_opt);
       // For Jazzy compatibility in Message Filters API change