Rebase

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

Author: elsayedelsheikh

nav2_collision_monitor/CMakeLists.txt

@@ -16,6 +16,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()
 
@@ -53,6 +54,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
@@ -87,6 +89,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
@@ -168,6 +171,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,9 @@ class PointCloud : public Source
   // ----- Variables -----
 
   /// @brief PointCloud data subscriber
-  nav2::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr data_sub_;
+  std::shared_ptr<point_cloud_transport::PointCloudTransport> pct_;
+  point_cloud_transport::Subscriber data_sub_;
+  std::string transport_type_;
 
   // Minimum and maximum height of PointCloud projected to 2D space
   double min_height_, max_height_;

nav2_collision_monitor/package.xml

@@ -24,6 +24,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,7 @@ PointCloud::PointCloud(
 PointCloud::~PointCloud()
 {
   RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str());
-  data_sub_.reset();
+  data_sub_.shutdown();
 }
 
 void PointCloud::configure()
@@ -60,10 +60,15 @@ void PointCloud::configure()
 
   getParameters(source_topic);
 
-  data_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
-    source_topic,
+  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),
-    nav2::qos::SensorDataQoS());
+    {}, &hint
+  );
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
@@ -131,6 +136,8 @@ 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();
+    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)