From 680f9ec477daffc5b705c2eb7ec4e83f0b50627d Mon Sep 17 00:00:00 2001
From: Kenji Brameld <kbrameld@traclabs.com>
Date: Tue, 19 Aug 2025 18:27:39 +0000
Subject: [PATCH] use point cloud transport to allow compressed point clouds to
 be sent across topics

---
 realsense2_camera/CMakeLists.txt              | 9 ++++++---
 realsense2_camera/include/pointcloud_filter.h | 9 ++++++---
 realsense2_camera/package.xml                 | 7 ++++---
 realsense2_camera/src/pointcloud_filter.cpp   | 8 ++++----
 4 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt
index f8a66a6a1..f1ab891b0 100644
--- a/realsense2_camera/CMakeLists.txt
+++ b/realsense2_camera/CMakeLists.txt
@@ -120,6 +120,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
+find_package(point_cloud_transport REQUIRED)
 
 find_package(realsense2 2.56)
 if (BUILD_ACCELERATE_GPU_WITH_GLSL)
@@ -263,6 +264,7 @@ set(dependencies
   tf2
   tf2_ros
   diagnostic_updater
+  point_cloud_transport
 )
 
 set (targets
@@ -278,6 +280,7 @@ set (targets
   tf2::tf2
   tf2_ros::tf2_ros
   diagnostic_updater::diagnostic_updater
+  point_cloud_transport::point_cloud_transport
 )
 
 if (BUILD_ACCELERATE_GPU_WITH_GLSL)
@@ -293,7 +296,7 @@ if(USE_LIFECYCLE_NODE)
     find_package(rclcpp_lifecycle REQUIRED)
     find_package(lifecycle_msgs REQUIRED)
     list(APPEND dependencies rclcpp_lifecycle lifecycle_msgs)
-    list(APPEND targets 
+    list(APPEND targets
         rclcpp_lifecycle::rclcpp_lifecycle
         lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_cpp)
     add_definitions(-DUSE_LIFECYCLE_NODE)
@@ -342,13 +345,13 @@ install(
 )
 
 # Install launch files
-install(DIRECTORY 
+install(DIRECTORY
     launch
     DESTINATION share/${PROJECT_NAME}
     )
 
 # Install example files
-install(DIRECTORY 
+install(DIRECTORY
     examples
     DESTINATION share/${PROJECT_NAME}
 )
diff --git a/realsense2_camera/include/pointcloud_filter.h b/realsense2_camera/include/pointcloud_filter.h
index 94c8b0896..81fdc47b9 100644
--- a/realsense2_camera/include/pointcloud_filter.h
+++ b/realsense2_camera/include/pointcloud_filter.h
@@ -17,9 +17,11 @@
 #include <string>
 #include <memory>
 #include <librealsense2/rs.hpp>
+#include <point_cloud_transport/point_cloud_transport.hpp>
+#include <point_cloud_transport/publisher.hpp>
+#include <ros_sensor.h>
 #include <sensor_params.h>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <ros_sensor.h>
 #include "named_filter.h"
 
 namespace realsense2_camera
@@ -28,7 +30,7 @@ namespace realsense2_camera
     {
         public:
             PointcloudFilter(std::shared_ptr<rs2::filter> filter, RosNodeBase& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false);
-        
+
             void setPublisher();
             void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id);
 
@@ -41,7 +43,8 @@ namespace realsense2_camera
             bool _allow_no_texture_points;
             bool _ordered_pc;
             std::mutex _mutex_publisher;
-            rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _pointcloud_publisher;
+            std::shared_ptr<point_cloud_transport::PointCloudTransport> _pct;
+            std::shared_ptr<point_cloud_transport::Publisher> _pointcloud_publisher;
             std::string _pointcloud_qos;
     };
 }
diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml
index 44a8b4d98..a7b7622d9 100644
--- a/realsense2_camera/package.xml
+++ b/realsense2_camera/package.xml
@@ -6,12 +6,12 @@
   <description>RealSense camera package allowing access to Intel D400 3D cameras</description>
   <maintainer email="librs.ros@intel.com">LibRealSense ROS Team</maintainer>
   <license>Apache License 2.0</license>
-  
+
   <url type="website">http://www.ros.org/wiki/RealSense</url>
   <url type="bugtracker">https://github.com/intel-ros/realsense/issues</url>
 
   <author email="librs.ros@intel.com">LibRealSense ROS Team</author>
- 
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>eigen</depend>
   <depend>builtin_interfaces</depend>
@@ -34,6 +34,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>diagnostic_updater</depend>
+  <depend>point_cloud_transport</depend>
   <test_depend condition="$ROS_DISTRO != foxy">ament_cmake_gtest</test_depend>
   <test_depend condition="$ROS_DISTRO != foxy">launch_testing</test_depend>
   <test_depend condition="$ROS_DISTRO != foxy">ament_cmake_pytest</test_depend>
@@ -45,7 +46,7 @@
   <test_depend condition="$ROS_DISTRO != foxy">python3-requests</test_depend>
   <test_depend condition="$ROS_DISTRO != foxy">tf2_ros_py</test_depend>
   <test_depend condition="$ROS_DISTRO != foxy">ros2topic</test_depend>
-  
+
   <exec_depend>launch_ros</exec_depend>
   <build_depend>ros_environment</build_depend>
 
diff --git a/realsense2_camera/src/pointcloud_filter.cpp b/realsense2_camera/src/pointcloud_filter.cpp
index 27c924542..4104c8bb9 100644
--- a/realsense2_camera/src/pointcloud_filter.cpp
+++ b/realsense2_camera/src/pointcloud_filter.cpp
@@ -69,9 +69,9 @@ void PointcloudFilter::setPublisher()
     std::lock_guard<std::mutex> lock_guard(_mutex_publisher);
     if ((_is_enabled) && (!_pointcloud_publisher))
     {
-        _pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("~/depth/color/points",
-                                rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)),
-                                            qos_string_to_qos(_pointcloud_qos)));
+        _pct = std::make_shared<point_cloud_transport::PointCloudTransport>(_node.shared_from_this());
+        _pointcloud_publisher = std::make_shared<point_cloud_transport::Publisher>(
+            _pct->advertise("~/depth/color/points", qos_string_to_qos(_pointcloud_qos)));
     }
     else if ((!_is_enabled) && (_pointcloud_publisher))
     {
@@ -92,7 +92,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
 {
     {
         std::lock_guard<std::mutex> lock_guard(_mutex_publisher);
-        if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count())))
+        if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->getNumSubscribers())))
             return;
     }
     rs2_stream texture_source_id = static_cast<rs2_stream>(_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER));