managed pc fixes, move to cuda pub

Signed-off-by: Max SCHMELLER <[email protected]>

nebula_decoders/include/nebula_decoders/nebula_decoders_common/managed_point_cloud.hpp

@@ -70,14 +70,14 @@ class ManagedPointCloud
       cloud_.data.resize(new_size);
       cloud_.width += 1;
       cloud_.row_step = cloud_.width * cloud_.point_step;
-      (*this)[index] = p;
+      set(index, p);
       return;
     }
 
     uint16_t channel = p.channel;
     auto & row_end = organized_metadata_->row_ends.at(channel);
     size_t index = channel * cloud_.width + row_end;
-    (*this)[index] = p;
+    set(index, p);
     row_end++;
   }
 
@@ -87,6 +87,8 @@ class ManagedPointCloud
    */
   void reset()
   {
+    resets++;
+
     if (!organized_metadata_.has_value()) {
       cloud_.width = cloud_.row_step = 0;
       cloud_.data.clear();
@@ -142,7 +144,15 @@ class ManagedPointCloud
   sensor_msgs::msg::PointCloud2 pop_cloud()
   {
     sensor_msgs::msg::PointCloud2 result;
-    std::swap(result, cloud_);
+    result.header = cloud_.header;
+    result.point_step = cloud_.point_step;
+    result.row_step = cloud_.row_step;
+    result.width = cloud_.width;
+    result.height = cloud_.height;
+    result.fields = cloud_.fields;
+    result.is_bigendian = cloud_.is_bigendian;
+    result.is_dense = cloud_.is_dense;
+    std::swap(result.data, cloud_.data);
     cloud_.data.clear();
     reset();
     return result;
@@ -156,6 +166,11 @@ class ManagedPointCloud
   const sensor_msgs::msg::PointCloud2 & peek_cloud() { return cloud_; }
 
 private:
+  void set(size_t index, const NebulaPoint & value)
+  {
+    std::memcpy(&cloud_.data[index * sizeof(value)], &value, sizeof(value));
+  }
+
   NebulaPoint & operator[](size_t index)
   {
     return *reinterpret_cast<NebulaPoint *>(&cloud_.data[index * sizeof(NebulaPoint)]);
@@ -171,6 +186,8 @@ class ManagedPointCloud
 
   sensor_msgs::msg::PointCloud2 cloud_;
   std::optional<OrganizedCloudMetadata> organized_metadata_;
+
+  size_t resets = 0;
 };
 
 }  // namespace nebula::drivers

nebula_ros/CMakeLists.txt

@@ -7,7 +7,7 @@ if (NOT CMAKE_CXX_STANDARD)
 endif ()
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function)
+    add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function -Wl,--no-undefined -Wl,-z,defs)
 endif ()
 
 find_package(ament_cmake_auto REQUIRED)
@@ -62,6 +62,7 @@ target_include_directories(hesai_ros_wrapper PUBLIC
     ${pandar_msgs_INCLUDE_DIRS}
     ${CUDA_INCLUDE_DIRS}
     ${cuda_blackboard_INCLUDE_DIRS}
+    ${negotiated_INCLUDE_DIRS}
 )
 
 target_link_libraries(hesai_ros_wrapper PUBLIC
@@ -70,6 +71,7 @@ target_link_libraries(hesai_ros_wrapper PUBLIC
     ${pandar_msgs_TARGETS}
     ${CUDA_LIBRARIES}
     ${cuda_blackboard_TARGETS}
+    ${negotiated_TARGETS}
     nebula_decoders::nebula_decoders_hesai
     nebula_hw_interfaces::nebula_hw_interfaces_hesai
 )

nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml

@@ -26,4 +26,4 @@
     ptp_switch_type: TSN
     retry_hw: true
     dual_return_distance_threshold: 0.1
-    publish_to_gpu: false
+    publish_to_gpu: true

nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp

@@ -14,8 +14,9 @@
 
 #pragma once
 
+#include "cuda_blackboard/cuda_blackboard_publisher.hpp"
+#include "cuda_blackboard/cuda_pointcloud2.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp"
-#include "nebula_ros/common/pointcloud_publishers.hpp"
 #include "nebula_ros/common/watchdog_timer.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
@@ -79,7 +80,8 @@ class HesaiDecoderWrapper
   rclcpp::Publisher<pandar_msgs::msg::PandarScan>::SharedPtr packets_pub_{};
   pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{};
 
-  std::shared_ptr<PointCloudPublisher> nebula_points_pub_;
+  std::shared_ptr<cuda_blackboard::CudaBlackboardPublisher<cuda_blackboard::CudaPointCloud2>>
+    cloud_pub_;
 
   std::shared_ptr<WatchdogTimer> cloud_watchdog_;
 };

nebula_ros/package.xml

@@ -23,6 +23,7 @@
   <depend>nebula_decoders</depend>
   <depend>nebula_hw_interfaces</depend>
   <depend>nebula_msgs</depend>
+  <depend>negotiated</depend>
   <depend>pandar_msgs</depend>
   <depend>radar_msgs</depend>
   <depend>rclcpp</depend>

nebula_ros/src/hesai/decoder_wrapper.cpp

@@ -4,7 +4,6 @@
 
 #include "cuda_blackboard/cuda_blackboard_publisher.hpp"
 #include "cuda_blackboard/cuda_pointcloud2.hpp"
-#include "nebula_ros/common/pointcloud_publishers.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <rclcpp/logging.hpp>
@@ -63,20 +62,9 @@ HesaiDecoderWrapper::HesaiDecoderWrapper(
       "pandar_packets", rclcpp::SensorDataQoS());
   }
 
-  auto qos_profile = rmw_qos_profile_sensor_data;
-  auto pointcloud_qos =
-    rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile);
-
-  if (config->publish_to_gpu) {
-    auto cuda_pub =
-      std::make_shared<cuda_blackboard::CudaBlackboardPublisher<cuda_blackboard::CudaPointCloud2>>(
-        parent_node_, "pandar_points");
-    nebula_points_pub_ = std::make_shared<CudaPointCloudPublisher>(cuda_pub);
-  } else {
-    auto pub =
-      parent_node_.create_publisher<sensor_msgs::msg::PointCloud2>("pandar_points", pointcloud_qos);
-    nebula_points_pub_ = std::make_shared<BasicPointCloudPublisher>(pub);
-  }
+  cloud_pub_ =
+    std::make_shared<cuda_blackboard::CudaBlackboardPublisher<cuda_blackboard::CudaPointCloud2>>(
+      parent_node_, "pandar_points");
 
   RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_);
 
@@ -151,7 +139,12 @@ void HesaiDecoderWrapper::ProcessCloudPacket(
     RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source.");
   }
 
-  nebula_points_pub_->publish_if_subscribers_exist(std::move(pointcloud));
+  if (
+    !cloud_pub_->get_subscription_count() && !cloud_pub_->get_intra_process_subscription_count()) {
+    return;
+  }
+
+  cloud_pub_->publish(std::move(pointcloud));
 }
 
 nebula::Status HesaiDecoderWrapper::Status()

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -22,7 +22,7 @@ namespace nebula
 namespace ros
 {
 HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
-: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)),
+: rclcpp::Node("hesai_ros_wrapper", options),
   wrapper_status_(Status::NOT_INITIALIZED),
   sensor_cfg_ptr_(nullptr),
   packet_queue_(3000),