From 7a167e08386ed0a7819c6a0db150f041acf5773d Mon Sep 17 00:00:00 2001 From: ElSayed ElSheikh Date: Wed, 30 Jul 2025 12:00:32 +0300 Subject: [PATCH 1/2] Include PointCloudTranport on Subscription Signed-off-by: ElSayed ElSheikh --- spatio_temporal_voxel_layer/CMakeLists.txt | 3 +++ .../example/constrained_indoor_environment_config.yaml | 2 ++ .../example/outdoor_environment_config.yaml | 2 ++ .../example/standard_indoor_environment_config.yaml | 2 ++ spatio_temporal_voxel_layer/example/vlp16_config.yaml | 2 ++ spatio_temporal_voxel_layer/package.xml | 2 ++ .../src/spatio_temporal_voxel_layer.cpp | 9 ++++++--- 7 files changed, 19 insertions(+), 3 deletions(-) diff --git a/spatio_temporal_voxel_layer/CMakeLists.txt b/spatio_temporal_voxel_layer/CMakeLists.txt index 45da324f..ac301872 100644 --- a/spatio_temporal_voxel_layer/CMakeLists.txt +++ b/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 diff --git a/spatio_temporal_voxel_layer/example/constrained_indoor_environment_config.yaml b/spatio_temporal_voxel_layer/example/constrained_indoor_environment_config.yaml index 84851823..75ad82c6 100644 --- a/spatio_temporal_voxel_layer/example/constrained_indoor_environment_config.yaml +++ b/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 diff --git a/spatio_temporal_voxel_layer/example/outdoor_environment_config.yaml b/spatio_temporal_voxel_layer/example/outdoor_environment_config.yaml index ce7aeeed..6caf4f58 100644 --- a/spatio_temporal_voxel_layer/example/outdoor_environment_config.yaml +++ b/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 diff --git a/spatio_temporal_voxel_layer/example/standard_indoor_environment_config.yaml b/spatio_temporal_voxel_layer/example/standard_indoor_environment_config.yaml index 6cac9540..97532bf2 100644 --- a/spatio_temporal_voxel_layer/example/standard_indoor_environment_config.yaml +++ b/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 diff --git a/spatio_temporal_voxel_layer/example/vlp16_config.yaml b/spatio_temporal_voxel_layer/example/vlp16_config.yaml index 2f73655a..6f095632 100644 --- a/spatio_temporal_voxel_layer/example/vlp16_config.yaml +++ b/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 diff --git a/spatio_temporal_voxel_layer/package.xml b/spatio_temporal_voxel_layer/package.xml index 9340c12f..1bc636e9 100644 --- a/spatio_temporal_voxel_layer/package.xml +++ b/spatio_temporal_voxel_layer/package.xml @@ -33,6 +33,8 @@ openvdb_vendor libopenexr-dev nav2_ros_common + point_cloud_transport + point_cloud_transport_plugins ament_lint_auto rosidl_interface_packages diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index fc4dea65..d2a87282 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -43,6 +43,7 @@ #include #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>( - node, topic, custom_qos_profile, sub_opt); + auto sub = std::make_shared( + *node, topic, transport_type, custom_qos_profile, sub_opt); sub->unsubscribe(); std::shared_ptr From 647e309c29b982f2587a4e008e1b37df7333569a Mon Sep 17 00:00:00 2001 From: ElSayed ElSheikh Date: Wed, 30 Jul 2025 12:06:37 +0300 Subject: [PATCH 2/2] Update README Signed-off-by: ElSayed ElSheikh --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 1f00929b..d08160cb 100644 --- a/README.md +++ b/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