Include PointCloudTranport on Subscription

README.md

@@ -125,6 +125,7 @@ rgbd_obstacle_layer:
   rgbd1_mark:
     data_type: PointCloud2
     topic: camera1/depth/points
+    transport_type: "raw"        #default raw -No compression-, Options: zlib, draco, zstd
     marking: true
     clearing: false
     min_obstacle_height: 0.3     #default 0, meters
@@ -139,6 +140,7 @@ rgbd_obstacle_layer:
     enabled: true                #default true, can be toggled on/off with associated service call
     data_type: PointCloud2
     topic: camera1/depth/points
+    transport_type: "raw"        #default raw -No compression-, Options: zlib, draco, zstd
     marking: false
     clearing: true
     min_z: 0.1                   #default 0, meters

spatio_temporal_voxel_layer/CMakeLists.txt

@@ -30,6 +30,7 @@ find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(openvdb_vendor REQUIRED)
+find_package(point_cloud_transport REQUIRED)
 
 remove_definitions(-DDISABLE_LIBUSB-1.0)
 find_package(Eigen3 REQUIRED)
@@ -69,6 +70,7 @@ set(dependencies
   visualization_msgs
   builtin_interfaces
   openvdb_vendor
+  point_cloud_transport
 )
 
 set(library_name spatio_temporal_voxel_layer_core)
@@ -118,6 +120,7 @@ target_link_libraries(${library_name}
   laser_geometry::laser_geometry
   pcl_conversions::pcl_conversions
   nav2_ros_common::nav2_ros_common
+  point_cloud_transport::point_cloud_transport
 )
 
 # Get a linker error when there are undefined symbols

spatio_temporal_voxel_layer/example/constrained_indoor_environment_config.yaml

@@ -20,6 +20,7 @@ global_costmap:
         rgbd1_mark:
           data_type: PointCloud2
           topic: camera1/depth/points
+          transport_type: "raw"        # default raw -No compression-, Options: zlib, draco, zstd
           marking: true
           clearing: false
           obstacle_range: 3.0          # meters
@@ -34,6 +35,7 @@ global_costmap:
         rgbd1_clear:
           data_type: PointCloud2
           topic: camera1/depth/points
+          transport_type: "raw"       # default raw -No compression-, Options: zlib, draco, zstd
           marking: false
           clearing: true
           max_z: 7.0                  # default 0, meters

spatio_temporal_voxel_layer/example/outdoor_environment_config.yaml

@@ -20,6 +20,7 @@ global_costmap:
         rgbd1_mark:
           data_type: PointCloud2
           topic: camera1/depth/points
+          transport_type: "raw"        # default raw -No compression-, Options: zlib, draco, zstd
           marking: true
           clearing: false
           obstacle_range: 3.0          # meters
@@ -34,6 +35,7 @@ global_costmap:
         rgbd1_clear:
           data_type: PointCloud2
           topic: camera1/depth/points
+          transport_type: "raw"        # default raw -No compression-, Options: zlib, draco, zstd
           marking: false
           clearing: true
           max_z: 7.0                  # default 0, meters

spatio_temporal_voxel_layer/example/standard_indoor_environment_config.yaml

@@ -20,6 +20,7 @@ global_costmap:
         rgbd1_mark:
           data_type: PointCloud2
           topic: camera/depth/points
+          transport_type: "raw"        # default raw -No compression-, Options: zlib, draco, zstd
           marking: true
           clearing: false
           obstacle_range: 3.0          # meters
@@ -34,6 +35,7 @@ global_costmap:
         rgbd1_clear:
           data_type: PointCloud2
           topic: camera/depth/points
+          transport_type: "raw"       # default raw -No compression-, Options: zlib, draco, zstd
           marking: false
           clearing: true
           max_z: 7.0                  # default 0, meters

spatio_temporal_voxel_layer/example/vlp16_config.yaml

@@ -20,6 +20,7 @@ global_costmap:
         rgbd1_mark:
           data_type: PointCloud2
           topic: /velodyne_points
+          transport_type: "raw"        # default raw -No compression-, Options: zlib, draco, zstd
           marking: true
           clearing: false
           obstacle_range: 3.0          # meters
@@ -34,6 +35,7 @@ global_costmap:
           enabled: true                #default true, can be toggled on/off with associated service call
           data_type: PointCloud2
           topic: /velodyne_points
+          transport_type: "raw"       # default raw -No compression-, Options: zlib, draco, zstd
           marking: false
           clearing: true
           max_z: 8.0                  # default 0, meters

spatio_temporal_voxel_layer/package.xml

@@ -33,6 +33,8 @@
   <depend>openvdb_vendor</depend>
   <depend>libopenexr-dev</depend>
   <depend>nav2_ros_common</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>point_cloud_transport_plugins</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

@@ -43,6 +43,7 @@
 #include <vector>
 
 #include "spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp"
+#include "point_cloud_transport/subscriber_filter.hpp"
 
 namespace spatio_temporal_voxel_layer
 {
@@ -172,7 +173,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
     double min_z, max_z, vFOV, vFOVPadding;
     double hFOV, decay_acceleration, obstacle_range;
-    std::string topic, sensor_frame, data_type, filter_str;
+    std::string topic, sensor_frame, data_type, filter_str, transport_type;
     bool inf_is_valid = false, clearing, marking;
     bool clear_after_reading, enabled;
     int voxel_min_points;
@@ -185,6 +186,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     declareParameter(
       source + "." + "data_type",
       rclcpp::ParameterValue(std::string("PointCloud2")));
+    declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw")));
     declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
     declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(3.0));
     declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
@@ -213,6 +215,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
       name_ + "." + source + "." + "expected_update_rate",
       expected_update_rate);
     node->get_parameter(name_ + "." + source + "." + "data_type", data_type);
+    node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type);
     node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height);
     node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height);
     node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
@@ -317,8 +320,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
 
       _observation_notifiers.back()->setTolerance(rclcpp::Duration::from_seconds(0.05));
     } else if (data_type == "PointCloud2") {
-      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(
-        node, topic, custom_qos_profile, sub_opt);
+      auto sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
+        *node, topic, transport_type, custom_qos_profile, sub_opt);
       sub->unsubscribe();
 
       std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>