pointcloud transport

Author: amilearning

elevation_mapping_cupy/CMakeLists.txt

@@ -40,7 +40,7 @@ find_package(pcl_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(python_cmake_module REQUIRED)
-
+find_package(point_cloud_transport REQUIRED)
 
 _ament_cmake_python_register_environment_hook()
 ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR script/${PROJECT_NAME})
@@ -61,6 +61,7 @@ set(dependencies
   pcl_ros
   message_filters
   tf2_eigen
+  point_cloud_transport
 )
 
 # Include directories

elevation_mapping_cupy/config/core/core_param.yaml

@@ -109,7 +109,8 @@ elevation_mapping:
     subscribers:
       pointcloud1:
         # topic_name: '/ouster/points'      
-        topic_name: '/camera/camera/depth/color/points'    
+        # topic_name: '/left/zed_node_1/point_cloud/cloud_registered'            
+        topic_name: '/left/zed_node/point_cloud/cloud_registered'            
         data_type: 'pointcloud'
       image1:
         topic_name: '/feat_processing_node/semantic_feat'

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -15,8 +15,9 @@
 // Pybind
 #include <pybind11/embed.h>  // everything needed for embedding
 #include <rclcpp/qos.hpp>
-// ROS
+// ROS2
 #include <geometry_msgs/msg/polygon_stamped.hpp>
+#include <point_cloud_transport/point_cloud_transport.hpp>
 #include <image_transport/image_transport.hpp>
 #include <image_transport/subscriber_filter.hpp>
 #include "message_filters/subscriber.h"
@@ -100,7 +101,9 @@ class ElevationMappingNode : public rclcpp::Node {
 void readParameters();
 void setupMapPublishers();
 void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key);    
-void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector<std::string>& channels);
+void pointcloudtransportCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const std::string& key);    
+
+void inputPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const std::vector<std::string>& channels);
 
 void inputImage(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg,
                   const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg,
@@ -146,6 +149,9 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start
   // image_transport::ImageTransport it_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr> pointcloudSubs_;
 
+  // std::vector<point_cloud_transport::Subscriber>::SharedPtr> pointcloudtransportSubs_;
+  std::vector<point_cloud_transport::Subscriber> pointcloudtransportSubs_;
+
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> imageSubs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> cameraInfoSubs_;
   std::vector<rclcpp::Subscription<elevation_map_msgs::msg::ChannelInfo>::SharedPtr> channelInfoSubs_;

elevation_mapping_cupy/package.xml

@@ -15,6 +15,7 @@
     <depend>std_srvs</depend>    
     <depend>sensor_msgs</depend>
     <depend>geometry_msgs</depend>
+    <depend>point_cloud_transport</depend>    
 
     <depend>message_filters</depend>
     <depend>grid_map_msgs</depend>

elevation_mapping_cupy/src/elevation_mapping_ros.cpp

@@ -189,11 +189,40 @@ ElevationMappingNode::ElevationMappingNode()
             auto sensor_qos = rclcpp::QoS(rclcpp::QoSInitialization(sensor_qos_profile.history, sensor_qos_profile.depth), sensor_qos_profile);
 
 
-            auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
-                  this->pointcloudCallback(msg, key);
-              };
-              auto sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(pointcloud_topic, sensor_qos, callback);
-              pointcloudSubs_.push_back(sub);
+            // point_cloud_transport::Subscriber pct_sub = pct.subscribe(
+            //     "pct/point_cloud", 100,
+            //     [node](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg)
+            //     {
+            //       RCLCPP_INFO_STREAM(
+            //         node->get_logger(),
+            //         "Message received, number of points is: " << msg->width * msg->height);
+            //     }, {});
+            // point_cloud_transport::Subscriber sub = pct.subscribe(pointcloud_topic, 100, callback, {}, transport_hint.get())
+
+
+            auto callback_transport = [this, key](const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) {
+            this->pointcloudtransportCallback(msg, key);
+            };
+
+            // Create transport hints (e.g., "draco")
+            // auto transport_hint = std::make_shared<point_cloud_transport::TransportHints>("draco");
+            
+
+            // Use PointCloudTransport to create a subscriber
+            point_cloud_transport::PointCloudTransport pct(node_);
+            auto sub_transport = pct.subscribe(pointcloud_topic, 100, callback_transport);
+
+            // Add the subscriber to the vector to manage its lifetime
+            pointcloudtransportSubs_.push_back(sub_transport);
+
+
+            // auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
+            //       this->pointcloudCallback(msg, key);
+            //   };
+              
+            //   auto sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(pointcloud_topic, sensor_qos, callback);
+            
+            //   pointcloudSubs_.push_back(sub);
                 RCLCPP_INFO(this->get_logger(), "Subscribed to PointCloud2 topic: %s", pointcloud_topic.c_str());
           }
       }
@@ -400,6 +429,23 @@ imageChannelReady_[key] = std::make_pair(*channel_info, true);
     }
 }
 
+
+void ElevationMappingNode::pointcloudtransportCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const std::string& key) {
+  
+  //  get channels
+  auto fields = cloud->fields;
+  std::vector<std::string> channels;
+
+  for (size_t it = 0; it < fields.size(); it++) {
+      auto& field = fields[it];
+      channels.push_back(field.name);
+  }
+  inputPointCloud(cloud, channels);
+
+  // This is used for publishing as statistics.
+  pointCloudProcessCounter_++;
+}
+
 void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key) {
   //  get channels
   auto fields = cloud->fields;
@@ -415,7 +461,7 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud
   pointCloudProcessCounter_++;
 }
 
-void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud,
+void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud,
                                            const std::vector<std::string>& channels) {
     auto start = this->now();
     // auto* raw_pcl_pc = new pcl::PCLPointCloud2;
@@ -640,10 +686,13 @@ bool ElevationMappingNode::getSubmap(
   Eigen::Isometry3d transformationOdomToMap;
   geometry_msgs::msg::Pose pose;          
   grid_map::Position requestedSubmapPosition(request->position_x, request->position_y);
+  
+  
   if (requestedFrameId != mapFrameId_) {
     geometry_msgs::msg::TransformStamped transformStamped;
-    const auto& timeStamp = this->now();
+    
     try {
+      const auto& timeStamp = this->now();
       transformStamped = tfBuffer_->lookupTransform(requestedFrameId, mapFrameId_, timeStamp, tf2::durationFromSec(1.0));      
 
 
@@ -691,6 +740,7 @@ bool ElevationMappingNode::getSubmap(
     response->map = *msg_ptr;
 
   }
+  
   return isSuccess;
 }
 

point_cloud_transport

@@ -0,0 +1 @@
+Subproject commit fd9eb0d371c62cf3155de9ea8e312df3beafc59a

point_cloud_transport_plugins

@@ -0,0 +1 @@
+Subproject commit f5005add81c3af44b33f884a7c99f87d407d70c7