Merge branch 'main' into clang-10

nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp

@@ -34,7 +34,7 @@ class RobosenseDriver : NebulaDriverBase
 {
 private:
   /// @brief Current driver status
-  Status driver_status_;
+  Status driver_status_{Status::NOT_INITIALIZED};
 
   /// @brief Decoder according to the model
   std::shared_ptr<RobosenseScanDecoder> scan_decoder_;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -85,7 +85,7 @@ int Vlp16Decoder::points_per_packet()
 
 void Vlp16Decoder::reset_pointcloud(double time_stamp)
 {
-  scan_pc_->points.clear();
+  scan_pc_->clear();
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
 }
 

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -83,8 +83,7 @@ int Vlp32Decoder::points_per_packet()
 
 void Vlp32Decoder::reset_pointcloud(double time_stamp)
 {
-  //  scan_pc_.reset(new NebulaPointCloud);
-  scan_pc_->points.clear();
+  scan_pc_->clear();
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
 }
 

nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

@@ -86,8 +86,7 @@ int Vls128Decoder::points_per_packet()
 
 void Vls128Decoder::reset_pointcloud(double time_stamp)
 {
-  //  scan_pc_.reset(new NebulaPointCloud);
-  scan_pc_->points.clear();
+  scan_pc_->clear();
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
 }
 

nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp

@@ -30,6 +30,7 @@
 #include <rclcpp_components/register_node_macro.hpp>
 
 #include <nebula_msgs/msg/nebula_packet.hpp>
+#include <robosense_msgs/msg/detail/robosense_info_packet__struct.hpp>
 #include <robosense_msgs/msg/robosense_info_packet.hpp>
 #include <robosense_msgs/msg/robosense_scan.hpp>
 
@@ -81,14 +82,17 @@ 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
   MtQueue<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::Publisher<robosense_msgs::msg::RobosenseInfoPacket>::SharedPtr info_packets_pub_;
+
+  rclcpp::Subscription<robosense_msgs::msg::RobosenseScan>::SharedPtr packets_sub_;
+  rclcpp::Subscription<robosense_msgs::msg::RobosenseInfoPacket>::SharedPtr info_packets_sub_;
 
   bool launch_hw_;
 

nebula_ros/src/hesai/hw_monitor_wrapper.cpp

@@ -4,6 +4,7 @@
 
 #include "nebula_ros/common/parameter_descriptors.hpp"
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <nebula_common/nebula_common.hpp>
 
 namespace nebula::ros
@@ -13,7 +14,8 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(
   const std::shared_ptr<nebula::drivers::HesaiHwInterface> & hw_interface,
   std::shared_ptr<const nebula::drivers::HesaiSensorConfiguration> & config)
 : logger_(parent_node->get_logger().get_child("HwMonitor")),
-  diagnostics_updater_(parent_node),
+  diagnostics_updater_(
+    (parent_node->declare_parameter<bool>("diagnostic_updater.use_fqn", true), parent_node)),
   status_(Status::OK),
   hw_interface_(hw_interface),
   parent_node_(parent_node)

nebula_ros/src/robosense/robosense_ros_wrapper.cpp

@@ -4,6 +4,13 @@
 
 #include "nebula_ros/common/parameter_descriptors.hpp"
 
+#include <rclcpp/qos.hpp>
+
+#include <robosense_msgs/msg/detail/robosense_info_packet__struct.hpp>
+
+#include <algorithm>
+#include <iterator>
+
 #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical"
 
 namespace nebula::ros
@@ -12,10 +19,7 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options)
 : rclcpp::Node("robosense_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)),
   wrapper_status_(Status::NOT_INITIALIZED),
   sensor_cfg_ptr_(nullptr),
-  packet_queue_(3000),
-  hw_interface_wrapper_(),
-  hw_monitor_wrapper_(),
-  decoder_wrapper_()
+  packet_queue_(3000)
 {
   setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 
@@ -28,23 +32,28 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options)
 
   RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_);
 
+  info_driver_.emplace(sensor_cfg_ptr_);
+
   launch_hw_ = declare_parameter<bool>("launch_hw", param_read_only());
 
   if (launch_hw_) {
     hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_);
     hw_monitor_wrapper_.emplace(this, sensor_cfg_ptr_);
-    info_driver_.emplace(sensor_cfg_ptr_);
   }
 
   RCLCPP_DEBUG(get_logger(), "Starting stream");
 
   decoder_thread_ = std::thread([this]() {
     while (true) {
+      if (!decoder_wrapper_) continue;
       decoder_wrapper_->process_cloud_packet(packet_queue_.pop());
     }
   });
 
   if (launch_hw_) {
+    info_packets_pub_ =
+      create_publisher<robosense_msgs::msg::RobosenseInfoPacket>("robosense_info_packets", 10);
+
     hw_interface_wrapper_->hw_interface()->register_scan_callback(
       std::bind(&RobosenseRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1));
     hw_interface_wrapper_->hw_interface()->register_info_callback(
@@ -54,9 +63,16 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options)
     packets_sub_ = create_subscription<robosense_msgs::msg::RobosenseScan>(
       "robosense_packets", rclcpp::SensorDataQoS(),
       std::bind(&RobosenseRosWrapper::receive_scan_message_callback, this, std::placeholders::_1));
+    info_packets_sub_ = create_subscription<robosense_msgs::msg::RobosenseInfoPacket>(
+      "robosense_info_packets", 10, [this](const robosense_msgs::msg::RobosenseInfoPacket & msg) {
+        std::vector<uint8_t> raw_packet;
+        std::copy(msg.packet.data.cbegin(), msg.packet.data.cend(), std::back_inserter(raw_packet));
+        receive_info_packet_callback(raw_packet);
+      });
     RCLCPP_INFO_STREAM(
-      get_logger(),
-      "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name());
+      get_logger(), "Hardware connection disabled, listening for packets on "
+                      << packets_sub_->get_topic_name() << " and "
+                      << info_packets_sub_->get_topic_name());
   }
 
   // Register parameter callback after all params have been declared. Otherwise it would be called
@@ -153,6 +169,18 @@ void RobosenseRosWrapper::receive_info_packet_callback(std::vector<uint8_t> & pa
       "Wrapper already receiving packets despite not being fully initialized yet.");
   }
 
+  if (info_packets_pub_) {
+    robosense_msgs::msg::RobosenseInfoPacket info_packet{};
+    if (packet.size() != info_packet.packet.data.size()) {
+      RCLCPP_ERROR_THROTTLE(
+        get_logger(), *get_clock(), 1000, "Could not publish info packet: size unsupported");
+    } else {
+      std::copy(packet.cbegin(), packet.cend(), info_packet.packet.data.begin());
+      info_packet.packet.stamp = now();
+      info_packets_pub_->publish(info_packet);
+    }
+  }
+
   auto status = info_driver_->decode_info_packet(packet);
 
   if (status != nebula::Status::OK) {

nebula_tests/data/hesai/ot128/1730271167765338806/metadata.yaml

@@ -0,0 +1,26 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: ""
+  duration:
+    nanoseconds: 98498380
+  starting_time:
+    nanoseconds_since_epoch: 1730271167765338806
+  message_count: 2
+  topics_with_message_count:
+    - topic_metadata:
+        name: /pandar_packets
+        type: pandar_msgs/msg/PandarScan
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 1\n  depth: 5\n  reliability: 2\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 2
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - 1730271167765338806_0.db3
+  files:
+    - path: 1730271167765338806_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1730271167765338806
+      duration:
+        nanoseconds: 98498380
+      message_count: 2

nebula_tests/data/hesai/qt128/1730273789074637152/metadata.yaml

@@ -0,0 +1,26 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: ""
+  duration:
+    nanoseconds: 100122457
+  starting_time:
+    nanoseconds_since_epoch: 1730273789074637152
+  message_count: 2
+  topics_with_message_count:
+    - topic_metadata:
+        name: /pandar_packets
+        type: pandar_msgs/msg/PandarScan
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 1\n  depth: 5\n  reliability: 2\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 2
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - 1730273789074637152_0.db3
+  files:
+    - path: 1730273789074637152_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1730273789074637152
+      duration:
+        nanoseconds: 100122457
+      message_count: 2

nebula_tests/hesai/hesai_ros_decoder_test_main.cpp

@@ -24,7 +24,7 @@
 namespace nebula::test
 {
 
-const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = {
+const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[8] = {
   {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "hesai", 0, 0.0, 0., 360., 0.3f,
    200.f},
   {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "hesai", 0, 0.0, 0., 360., 0.3f,
@@ -36,7 +36,11 @@ const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = {
   {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "hesai", 0, 0.0, 0., 360.,
    0.05f, 120.f},
   {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "hesai", 0, 0.0,
-   0., 360., 0.5f, 300.f}};
+   0., 360., 0.5f, 300.f},
+  {"PandarQT128", "LastStrongest", "PandarQT128.csv", "qt128/1730273789074637152", "hesai", 0, 0.0,
+   0., 360., 0.3f, 300.f},
+  {"Pandar128E4X", "LastStrongest", "Pandar128E4X.csv", "ot128/1730271167765338806", "hesai", 0,
+   0.0, 0., 360., 0.3f, 300.f}};
 
 // Compares geometrical output of decoder against pre-recorded reference pointcloud.
 TEST_P(DecoderTest, TestPcd)