perf(hesai): get rid of copy when replaying /pandar_packets

.cspell.json

@@ -28,6 +28,7 @@
     "horiz",
     "Idat",
     "ipaddr",
+    "libmsgsl",
     "manc",
     "mdeg",
     "memcpy",

nebula_decoders/CMakeLists.txt

@@ -26,6 +26,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(velodyne_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(PNG REQUIRED)
+find_package(Microsoft.GSL REQUIRED)
 
 # Check if pcl_conversions provides modern CMake targets (ROS 2 Jazzy and later)
 # or if we need to use legacy variables (ROS 2 Humble and earlier)
@@ -64,6 +65,7 @@ add_library(nebula_decoders_hesai SHARED
 target_link_libraries(nebula_decoders_hesai PUBLIC
     ${pandar_msgs_TARGETS}
     ${PNG_LIBRARIES}
+    Microsoft.GSL::GSL
 )
 
 target_include_directories(nebula_decoders_hesai PUBLIC
@@ -177,6 +179,7 @@ ament_export_dependencies(
     sensor_msgs
     velodyne_msgs
     yaml-cpp
+    Microsoft.GSL
 )
 
 ament_package()

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -23,6 +23,7 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/packet_loss_detector.hpp"
 
+#include <gsl/span>
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/loggers/logger.hpp>
 #include <nebula_common/nebula_common.hpp>
@@ -106,7 +107,7 @@ class HesaiDecoder : public HesaiScanDecoder
   /// @brief Validates and parse PandarPacket. Checks size and, if present, CRC checksums.
   /// @param packet The incoming PandarPacket
   /// @return Whether the packet was parsed successfully
-  bool parse_packet(const std::vector<uint8_t> & packet)
+  bool parse_packet(gsl::span<const uint8_t> packet)
   {
     if (packet.size() < sizeof(typename SensorT::packet_t)) {
       NEBULA_LOG_STREAM(
@@ -350,7 +351,7 @@ class HesaiDecoder : public HesaiScanDecoder
     pointcloud_callback_ = std::move(callback);
   }
 
-  PacketDecodeResult unpack(const std::vector<uint8_t> & packet) override
+  PacketDecodeResult unpack(gsl::span<const uint8_t> packet) override
   {
     util::Stopwatch decode_watch;
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp

@@ -15,6 +15,7 @@
 #ifndef NEBULA_WS_HESAI_SCAN_DECODER_HPP
 #define NEBULA_WS_HESAI_SCAN_DECODER_HPP
 
+#include <gsl/span>
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/point_types.hpp>
 #include <nebula_common/util/expected.hpp>
@@ -74,7 +75,7 @@ class HesaiScanDecoder
   /// @param packet The incoming PandarPacket
   /// @return Metadata on success, or decode error on failure. Performance counters are always
   /// returned.
-  virtual PacketDecodeResult unpack(const std::vector<uint8_t> & packet) = 0;
+  virtual PacketDecodeResult unpack(gsl::span<const uint8_t> packet) = 0;
 
   virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0;
 };

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -24,6 +24,7 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/packet_loss_detector.hpp"
 
+#include <gsl/span>
 #include <nebula_common/loggers/logger.hpp>
 
 #include <pcl_conversions/pcl_conversions.h>
@@ -140,7 +141,7 @@ class HesaiDriver
   /// @param packet Packet to decode
   /// @return Metadata on success, or decode error on failure. Performance counters are always
   /// returned.
-  PacketDecodeResult parse_cloud_packet(const std::vector<uint8_t> & packet);
+  PacketDecodeResult parse_cloud_packet(gsl::span<const uint8_t> packet);
 };
 
 }  // namespace nebula::drivers

nebula_decoders/package.xml

@@ -21,6 +21,7 @@
   <depend>angles</depend>
   <depend>continental_msgs</depend>
   <depend>diagnostic_msgs</depend>
+  <depend>libmsgsl-dev</depend>
   <depend>nebula_common</depend>
   <depend>nebula_msgs</depend>
   <depend>pandar_msgs</depend>

nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp

@@ -16,6 +16,8 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp"
 
+#include <gsl/span>
+
 #include <cstdint>
 #include <memory>
 #include <utility>
@@ -116,7 +118,7 @@ std::shared_ptr<HesaiScanDecoder> HesaiDriver::initialize_decoder(
     std::move(blockage_mask_plugin));
 }
 
-PacketDecodeResult HesaiDriver::parse_cloud_packet(const std::vector<uint8_t> & packet)
+PacketDecodeResult HesaiDriver::parse_cloud_packet(gsl::span<const uint8_t> packet)
 {
   if (driver_status_ != nebula::Status::OK) {
     logger_->error("Driver not OK.");

nebula_ros/CMakeLists.txt

@@ -34,6 +34,7 @@ find_package(message_filters REQUIRED)
 find_package(autoware_utils_debug REQUIRED)
 find_package(autoware_internal_debug_msgs REQUIRED)
 find_package(agnocastlib)
+find_package(Microsoft.GSL REQUIRED)
 
 if("$ENV{ENABLE_AGNOCAST}" AND NOT agnocastlib_FOUND)
     message(FATAL_ERROR "agnocastlib is required when Agnocast is enabled")
@@ -82,6 +83,7 @@ target_link_libraries(hesai_ros_wrapper PUBLIC
     ${autoware_internal_debug_msgs_TARGETS}
     nebula_decoders::nebula_decoders_hesai
     nebula_hw_interfaces::nebula_hw_interfaces_hesai
+    Microsoft.GSL::GSL
 )
 
 if("$ENV{ENABLE_AGNOCAST}")
@@ -331,6 +333,7 @@ ament_export_dependencies(
     sync_tooling_msgs
     autoware_utils_debug
     autoware_internal_debug_msgs
+    Microsoft.GSL
 )
 
 if("$ENV{ENABLE_AGNOCAST}")

nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp

@@ -25,12 +25,12 @@
 #include <autoware_utils_debug/debug_publisher.hpp>
 #include <diagnostic_updater/publisher.hpp>
 #include <diagnostic_updater/update_functions.hpp>
+#include <gsl/span>
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/nebula_common.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <nebula_msgs/msg/nebula_packet.hpp>
 #include <pandar_msgs/msg/pandar_scan.hpp>
 
 #include <limits>
@@ -51,11 +51,12 @@ class HesaiDecoderWrapper
     diagnostic_updater::Updater & diagnostic_updater, bool publish_packets);
 
   /// @brief Process a cloud packet and return metadata
-  /// @param packet_msg The packet to process
-  /// @param receive_metadata Performance metadata from packet reception
+  /// @param packet_data The raw packet data
+  /// @param packet_stamp_ns Timestamp of the packet in nanoseconds
+  /// @param receive_time_ns Performance metadata from packet reception
   /// @return Expected containing metadata on success, or decode error on failure
   drivers::PacketDecodeResult process_cloud_packet(
-    std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg, uint64_t receive_time_ns);
+    gsl::span<const uint8_t> packet_data, uint64_t packet_stamp_ns, uint64_t receive_time_ns);
 
   void on_pointcloud_decoded(const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s);
 

nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp

@@ -28,7 +28,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 
-#include <nebula_msgs/msg/nebula_packet.hpp>
 #include <pandar_msgs/msg/pandar_scan.hpp>
 
 #include <boost/algorithm/string/join.hpp>

nebula_ros/package.xml

@@ -24,6 +24,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
+  <depend>libmsgsl-dev</depend>
   <depend>message_filters</depend>
   <depend>nebula_common</depend>
   <depend>nebula_decoders</depend>

nebula_ros/src/hesai/decoder_wrapper.cpp

@@ -9,6 +9,8 @@
 #include "nebula_ros/hesai/diagnostics/functional_safety_basic.hpp"
 #include "nebula_ros/hesai/diagnostics/functional_safety_diagnostic_task.hpp"
 
+#include <builtin_interfaces/msg/time.hpp>
+#include <gsl/span>
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/util/string_conversions.hpp>
 #include <rclcpp/logging.hpp>
@@ -111,7 +113,7 @@ void HesaiDecoderWrapper::on_calibration_change(
 }
 
 drivers::PacketDecodeResult HesaiDecoderWrapper::process_cloud_packet(
-  std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg, uint64_t receive_time_ns)
+  gsl::span<const uint8_t> packet_data, uint64_t packet_stamp_ns, uint64_t receive_time_ns)
 {
   current_scan_perf_counters_.receive_time_current_scan_ns += receive_time_ns;
 
@@ -120,19 +122,20 @@ drivers::PacketDecodeResult HesaiDecoderWrapper::process_cloud_packet(
   // publish thread, negating the benefits of multi-threading.
   // Not checking is an okay trade-off for the time being.
   if (packets_pub_) {
+    rclcpp::Time packet_stamp(static_cast<int64_t>(packet_stamp_ns));
     if (current_scan_msg_->packets.size() == 0) {
-      current_scan_msg_->header.stamp = packet_msg->stamp;
+      current_scan_msg_->header.stamp = packet_stamp;
     }
 
     pandar_msgs::msg::PandarPacket pandar_packet_msg{};
-    pandar_packet_msg.stamp = packet_msg->stamp;
-    pandar_packet_msg.size = packet_msg->data.size();
-    std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin());
+    pandar_packet_msg.stamp = packet_stamp;
+    pandar_packet_msg.size = packet_data.size();
+    std::copy(packet_data.begin(), packet_data.end(), pandar_packet_msg.data.begin());
     current_scan_msg_->packets.emplace_back(pandar_packet_msg);
   }
 
   std::lock_guard lock(mtx_driver_ptr_);
-  auto decode_result = driver_ptr_->parse_cloud_packet(packet_msg->data);
+  auto decode_result = driver_ptr_->parse_cloud_packet(packet_data);
 
   current_scan_perf_counters_.decode_time_current_scan_ns +=
     decode_result.performance_counters.decode_time_ns;

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -4,6 +4,7 @@
 
 #include "nebula_ros/common/parameter_descriptors.hpp"
 
+#include <gsl/span>
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/nebula_common.hpp>
 #include <nebula_common/util/rate_limiter.hpp>
@@ -414,12 +415,13 @@ void HesaiRosWrapper::receive_scan_message_callback(
     return;
   }
 
-  for (auto & pkt : scan_msg->packets) {
-    auto nebula_pkt_ptr = std::make_unique<nebula_msgs::msg::NebulaPacket>();
-    nebula_pkt_ptr->stamp = pkt.stamp;
-    std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data));
+  for (const auto & pkt : scan_msg->packets) {
+    uint64_t packet_stamp_ns = static_cast<uint64_t>(pkt.stamp.sec) * 1'000'000'000ULL +
+                               static_cast<uint64_t>(pkt.stamp.nanosec);
+    gsl::span<const uint8_t> packet_data(pkt.data.data(), pkt.size);
 
-    decoder_wrapper_->process_cloud_packet(std::move(nebula_pkt_ptr), receive_watch.elapsed_ns());
+    decoder_wrapper_->process_cloud_packet(
+      packet_data, packet_stamp_ns, receive_watch.elapsed_ns());
     // This reset is placed at the end of the loop, so that in the first iteration, the possible
     // logging overhead from the statements before the loop is included in the measurement.
     receive_watch.reset();
@@ -578,13 +580,10 @@ void HesaiRosWrapper::receive_cloud_packet_callback(
   const auto timestamp_ns =
     std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
 
-  auto msg_ptr = std::make_unique<nebula_msgs::msg::NebulaPacket>();
-  msg_ptr->stamp.sec = static_cast<int>(timestamp_ns / 1'000'000'000);
-  msg_ptr->stamp.nanosec = static_cast<int>(timestamp_ns % 1'000'000'000);
-  msg_ptr->data = packet;
+  gsl::span<const uint8_t> packet_data(packet);
 
   auto decode_result = decoder_wrapper_->process_cloud_packet(
-    std::move(msg_ptr), receive_metadata.packet_perf_counters.receive_duration_ns);
+    packet_data, timestamp_ns, receive_metadata.packet_perf_counters.receive_duration_ns);
 
   if (
     decode_result.metadata_or_error.has_value() && sync_tooling_plugin_ &&