fix: lower packet loss to near-zero levels

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp

@@ -17,6 +17,7 @@
 #include <cstddef>
 #include <cstdint>
 #include <ctime>
+#include <stdexcept>
 
 namespace nebula
 {

nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp

@@ -13,6 +13,8 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp"
 
+#include <rclcpp/logging.hpp>
+
 // #define WITH_DEBUG_STD_COUT_HESAI_CLIENT // Use std::cout messages for debugging
 
 namespace nebula

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp

@@ -21,6 +21,7 @@
 
 #include <array>
 #include <cstdint>
+#include <iomanip>
 #include <ostream>
 #include <string>
 

nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp

@@ -16,21 +16,28 @@
 
 #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp"
 #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
+#include "nebula_ros/common/mt_queue.hpp"
 #include "nebula_ros/common/parameter_descriptors.hpp"
 #include "nebula_ros/common/watchdog_timer.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
 #include <nebula_common/nebula_common.hpp>
+#include <nebula_common/point_types.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "nebula_msgs/msg/detail/nebula_packets__struct.hpp"
 #include "nebula_msgs/msg/nebula_packet.hpp"
+#include "nebula_msgs/msg/nebula_packets.hpp"
 #include "pandar_msgs/msg/pandar_packet.hpp"
 #include "pandar_msgs/msg/pandar_scan.hpp"
 
 #include <filesystem>
 #include <memory>
 #include <mutex>
 #include <string>
+#include <thread>
+#include <tuple>
+#include <utility>
 #include <vector>
 
 namespace nebula
@@ -63,6 +70,13 @@ class HesaiDecoderWrapper
   nebula::Status Status();
 
 private:
+  struct PublishData
+  {
+    nebula_msgs::msg::NebulaPackets::UniquePtr packets;
+    drivers::NebulaPointCloudPtr cloud;
+    double cloud_timestamp_s;
+  };
+
   /// @brief Load calibration data from the best available source:
   /// 1. If sensor connected, download and save from sensor
   /// 2. If downloaded file available, load that file
@@ -74,6 +88,8 @@ class HesaiDecoderWrapper
   get_calibration_result_t GetCalibrationData(
     const std::string & calibration_file_path, bool ignore_others = false);
 
+  void publish(PublishData && data);
+
   void PublishCloud(
     std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
     const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher);
@@ -93,18 +109,21 @@ class HesaiDecoderWrapper
   const std::shared_ptr<nebula::drivers::HesaiHwInterface> hw_interface_;
   std::shared_ptr<const nebula::drivers::HesaiSensorConfiguration> sensor_cfg_;
 
-  std::string calibration_file_path_{};
-  std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> calibration_cfg_ptr_{};
+  std::string calibration_file_path_;
+  std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> calibration_cfg_ptr_;
 
-  std::shared_ptr<drivers::HesaiDriver> driver_ptr_{};
+  std::shared_ptr<drivers::HesaiDriver> driver_ptr_;
   std::mutex mtx_driver_ptr_;
 
-  rclcpp::Publisher<pandar_msgs::msg::PandarScan>::SharedPtr packets_pub_{};
-  pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{};
+  mt_queue<PublishData> publish_queue_;
+  std::thread pub_thread_;
+
+  rclcpp::Publisher<pandar_msgs::msg::PandarScan>::SharedPtr packets_pub_;
+  nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_;
 
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_{};
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_;
 
   std::shared_ptr<WatchdogTimer> cloud_watchdog_;
 };

nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp

@@ -18,8 +18,6 @@
 #include "nebula_common/hesai/hesai_common.hpp"
 #include "nebula_common/nebula_common.hpp"
 #include "nebula_common/nebula_status.hpp"
-#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
-#include "nebula_ros/common/mt_queue.hpp"
 #include "nebula_ros/common/parameter_descriptors.hpp"
 #include "nebula_ros/hesai/decoder_wrapper.hpp"
 #include "nebula_ros/hesai/hw_interface_wrapper.hpp"
@@ -83,11 +81,6 @@ class HesaiRosWrapper final : public rclcpp::Node
 
   std::shared_ptr<const nebula::drivers::HesaiSensorConfiguration> sensor_cfg_ptr_{};
 
-  /// @brief Stores received packets that have not been processed yet by the decoder thread
-  mt_queue<std::unique_ptr<nebula_msgs::msg::NebulaPacket>> packet_queue_;
-  /// @brief Thread to isolate decoding from receiving
-  std::thread decoder_thread_;
-
   rclcpp::Subscription<pandar_msgs::msg::PandarScan>::SharedPtr packets_sub_{};
 
   bool launch_hw_;

nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp

@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include "nebula_ros/common/mt_queue.hpp"
 #include "nebula_ros/common/watchdog_timer.hpp"
 
 #include <nebula_common/nebula_common.hpp>
@@ -24,6 +25,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <nebula_msgs/msg/nebula_packet.hpp>
+#include <nebula_msgs/msg/nebula_packets.hpp>
 #include <robosense_msgs/msg/robosense_scan.hpp>
 
 #include <chrono>
@@ -51,6 +53,15 @@ class RobosenseDecoderWrapper
   nebula::Status Status();
 
 private:
+  struct PublishData
+  {
+    nebula_msgs::msg::NebulaPackets::UniquePtr packets;
+    drivers::NebulaPointCloudPtr cloud;
+    double cloud_timestamp_s;
+  };
+
+  void publish(PublishData && data);
+
   void PublishCloud(
     std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
     const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher);
@@ -69,17 +80,20 @@ class RobosenseDecoderWrapper
 
   std::shared_ptr<nebula::drivers::RobosenseHwInterface> hw_interface_;
   std::shared_ptr<const drivers::RobosenseSensorConfiguration> sensor_cfg_;
-  std::shared_ptr<const drivers::RobosenseCalibrationConfiguration> calibration_cfg_ptr_{};
+  std::shared_ptr<const drivers::RobosenseCalibrationConfiguration> calibration_cfg_ptr_;
 
   std::shared_ptr<drivers::RobosenseDriver> driver_ptr_;
   std::mutex mtx_driver_ptr_;
 
-  rclcpp::Publisher<robosense_msgs::msg::RobosenseScan>::SharedPtr packets_pub_{};
-  robosense_msgs::msg::RobosenseScan::UniquePtr current_scan_msg_{};
+  rclcpp::Publisher<robosense_msgs::msg::RobosenseScan>::SharedPtr packets_pub_;
+  nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_;
+
+  mt_queue<PublishData> publish_queue_;
+  std::thread pub_thread_;
 
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_{};
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_;
 
   std::shared_ptr<WatchdogTimer> cloud_watchdog_;
 };

nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp

@@ -14,7 +14,6 @@
 
 #pragma once
 
-#include "nebula_ros/common/mt_queue.hpp"
 #include "nebula_ros/robosense/decoder_wrapper.hpp"
 #include "nebula_ros/robosense/hw_interface_wrapper.hpp"
 #include "nebula_ros/robosense/hw_monitor_wrapper.hpp"
@@ -83,14 +82,9 @@ class RobosenseRosWrapper final : public rclcpp::Node
 
   nebula::Status wrapper_status_;
 
-  std::shared_ptr<const nebula::drivers::RobosenseSensorConfiguration> sensor_cfg_ptr_{};
+  std::shared_ptr<const nebula::drivers::RobosenseSensorConfiguration> sensor_cfg_ptr_;
 
-  /// @brief Stores received packets that have not been processed yet by the decoder thread
-  mt_queue<std::unique_ptr<nebula_msgs::msg::NebulaPacket>> packet_queue_;
-  /// @brief Thread to isolate decoding from receiving
-  std::thread decoder_thread_;
-
-  rclcpp::Subscription<robosense_msgs::msg::RobosenseScan>::SharedPtr packets_sub_{};
+  rclcpp::Subscription<robosense_msgs::msg::RobosenseScan>::SharedPtr packets_sub_;
 
   bool launch_hw_;
 

nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp

@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include "nebula_ros/common/mt_queue.hpp"
 #include "nebula_ros/common/parameter_descriptors.hpp"
 #include "nebula_ros/common/watchdog_timer.hpp"
 
@@ -25,6 +26,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <nebula_msgs/msg/nebula_packet.hpp>
+#include <nebula_msgs/msg/nebula_packets.hpp>
 #include <velodyne_msgs/msg/velodyne_packet.hpp>
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
@@ -64,11 +66,20 @@ class VelodyneDecoderWrapper
   nebula::Status Status();
 
 private:
+  struct PublishData
+  {
+    nebula_msgs::msg::NebulaPackets::UniquePtr packets;
+    drivers::NebulaPointCloudPtr cloud;
+    double cloud_timestamp_s;
+  };
+
   /// @brief Load calibration data from file
   /// @param calibration_file_path The file to read from
   /// @return The calibration data if successful, or an error code if not
   get_calibration_result_t GetCalibrationData(const std::string & calibration_file_path);
 
+  void publish(PublishData && data);
+
   void PublishCloud(
     std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
     const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher);
@@ -88,18 +99,21 @@ class VelodyneDecoderWrapper
   const std::shared_ptr<nebula::drivers::VelodyneHwInterface> hw_interface_;
   std::shared_ptr<const nebula::drivers::VelodyneSensorConfiguration> sensor_cfg_;
 
-  std::string calibration_file_path_{};
-  std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> calibration_cfg_ptr_{};
+  std::string calibration_file_path_;
+  std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> calibration_cfg_ptr_;
 
-  std::shared_ptr<drivers::VelodyneDriver> driver_ptr_{};
+  std::shared_ptr<drivers::VelodyneDriver> driver_ptr_;
   std::mutex mtx_driver_ptr_;
 
-  rclcpp::Publisher<velodyne_msgs::msg::VelodyneScan>::SharedPtr packets_pub_{};
-  velodyne_msgs::msg::VelodyneScan::UniquePtr current_scan_msg_{};
+  mt_queue<PublishData> publish_queue_;
+  std::thread pub_thread_;
+
+  rclcpp::Publisher<velodyne_msgs::msg::VelodyneScan>::SharedPtr packets_pub_;
+  nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_;
 
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_{};
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_;
 
   std::shared_ptr<WatchdogTimer> cloud_watchdog_;
 };

nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp

@@ -14,7 +14,6 @@
 
 #pragma once
 
-#include "nebula_ros/common/mt_queue.hpp"
 #include "nebula_ros/common/parameter_descriptors.hpp"
 #include "nebula_ros/velodyne/decoder_wrapper.hpp"
 #include "nebula_ros/velodyne/hw_interface_wrapper.hpp"
@@ -84,11 +83,6 @@ class VelodyneRosWrapper final : public rclcpp::Node
 
   std::shared_ptr<const nebula::drivers::VelodyneSensorConfiguration> sensor_cfg_ptr_{};
 
-  /// @brief Stores received packets that have not been processed yet by the decoder thread
-  mt_queue<std::unique_ptr<nebula_msgs::msg::NebulaPacket>> packet_queue_;
-  /// @brief Thread to isolate decoding from receiving
-  std::thread decoder_thread_;
-
   rclcpp::Subscription<velodyne_msgs::msg::VelodyneScan>::SharedPtr packets_sub_{};
 
   bool launch_hw_;