Merge branch 'main' into perf/srr520-remove-mtqueue

.pre-commit-config.yaml

@@ -47,13 +47,13 @@ repos:
       - id: shellcheck
 
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.10.0-2
+    rev: v3.11.0-1
     hooks:
       - id: shfmt
         args: [-w, -s, -i=4]
 
   - repo: https://github.com/pycqa/isort
-    rev: 6.0.0
+    rev: 6.0.1
     hooks:
       - id: isort
 

nebula_common/package.xml

@@ -12,7 +12,8 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>ros_environment</buildtool_depend>
 
-  <depend>libpcl-all-dev</depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_export_depend>libpcl-all-dev</build_export_depend>
   <depend>nlohmann-json-dev</depend>
   <depend>yaml-cpp</depend>
 

nebula_decoders/package.xml

@@ -11,13 +11,16 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>ros_environment</buildtool_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>libpng++-dev</build_depend>
+  <build_depend>libpng-dev</build_depend>
+  <build_export_depend>libpcl-all-dev</build_export_depend>
+  <build_export_depend>libpng++-dev</build_export_depend>
+  <build_export_depend>libpng-dev</build_export_depend>
 
   <depend>angles</depend>
   <depend>continental_msgs</depend>
   <depend>diagnostic_msgs</depend>
-  <depend>libpcl-all-dev</depend>
-  <depend>libpng++-dev</depend>
-  <depend>libpng-dev</depend>
   <depend>nebula_common</depend>
   <depend>nebula_msgs</depend>
   <depend>pandar_msgs</depend>

nebula_examples/package.xml

@@ -11,8 +11,9 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>ros_environment</buildtool_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_export_depend>libpcl-all-dev</build_export_depend>
 
-  <depend>libpcl-all-dev</depend>
   <depend>nebula_common</depend>
   <depend>nebula_decoders</depend>
   <depend>nebula_ros</depend>

nebula_ros/CMakeLists.txt

@@ -228,7 +228,7 @@ if(BUILD_TESTING)
 
     ament_lint_auto_find_test_dependencies()
 
-    foreach(MODEL Pandar40P Pandar64 PandarQT64 PandarQT128 Pandar128E4X PandarAT128 PandarXT16 PandarXT32 PandarXT32M SRR520)
+    foreach(MODEL Pandar40P Pandar64 PandarQT64 PandarQT128 Pandar128E4X PandarAT128 PandarXT16 PandarXT32 PandarXT32M ARS548 SRR520 VLP16 VLP32 VLS128)
         string(TOLOWER ${MODEL}_smoke_test test_name)
         add_ros_test(
             test/smoke_test.py

nebula_ros/config/radar/continental/ARS548.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     host_ip: 10.13.1.166
-    sensor_ip: 10.13.1.114
+    sensor_ip: 10.13.1.113
     data_port: 42102
     frame_id: continental
     base_frame: base_link

nebula_ros/include/nebula_ros/continental/continental_ars548_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/continental/continental_ars548_decoder_wrapper.hpp"
 #include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp"
@@ -80,11 +79,6 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node
   std::shared_ptr<const drivers::continental_ars548::ContinentalARS548SensorConfiguration>
     config_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<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_sub_{};
 
   bool launch_hw_{};

nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp

@@ -54,9 +54,6 @@ class VelodyneHwMonitorWrapper
   /// @brief Callback of the timer for getting the current lidar status
   void on_velodyne_status_timer();
 
-  /// @brief Callback of the timer for getting the current lidar snapshot
-  void on_velodyne_snapshot_timer();
-
   /// @brief Callback of the timer for getting the current lidar status & updating the diagnostics
   void on_velodyne_diagnostics_timer();
 
@@ -250,10 +247,6 @@ class VelodyneHwMonitorWrapper
   /// @param diagnostics DiagnosticStatusWrapper
   void velodyne_check_laser_state(diagnostic_updater::DiagnosticStatusWrapper & diagnostics);
 
-  /// @brief Check the current snapshot for diagnostic_updater
-  /// @param diagnostics DiagnosticStatusWrapper
-  void velodyne_check_snapshot(diagnostic_updater::DiagnosticStatusWrapper & diagnostics);
-
   /// @brief Check the current states of motor & laser for diagnostic_updater
   /// @param diagnostics DiagnosticStatusWrapper
   void velodyne_check_status(diagnostic_updater::DiagnosticStatusWrapper & diagnostics);
@@ -282,17 +275,11 @@ class VelodyneHwMonitorWrapper
   uint16_t diag_span_;
   bool show_advanced_diagnostics_;
 
-  rclcpp::TimerBase::SharedPtr diagnostics_snapshot_timer_;
   rclcpp::TimerBase::SharedPtr diagnostics_update_timer_;
   rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_;
 
-  std::shared_ptr<std::string> current_snapshot_;
-  std::shared_ptr<boost::property_tree::ptree> current_snapshot_tree_;
   std::shared_ptr<boost::property_tree::ptree> current_diag_tree_;
-  std::shared_ptr<rclcpp::Time> current_snapshot_time_;
-  uint8_t current_diag_status_;
 
-  std::mutex mtx_snapshot_;
   std::mutex mtx_status_;
   std::mutex mtx_diag_;
 

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"
@@ -82,11 +81,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
-  MtQueue<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_;

nebula_ros/package.xml

@@ -12,13 +12,14 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>ros_environment</buildtool_depend>
   <buildtool_depend>ros_testing</buildtool_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_export_depend>libpcl-all-dev</build_export_depend>
 
   <depend>continental_msgs</depend>
   <depend>continental_srvs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
-  <depend>libpcl-all-dev</depend>
   <depend>nebula_common</depend>
   <depend>nebula_decoders</depend>
   <depend>nebula_hw_interfaces</depend>

nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp

@@ -29,10 +29,7 @@ namespace nebula::ros
 ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options)
 : rclcpp::Node(
     "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)),
-  wrapper_status_(Status::NOT_INITIALIZED),
-  packet_queue_(3000),
-  hw_interface_wrapper_(),
-  decoder_wrapper_()
+  wrapper_status_(Status::NOT_INITIALIZED)
 {
   setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 
@@ -54,12 +51,6 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio
 
   RCLCPP_DEBUG(get_logger(), "Starting stream");
 
-  decoder_thread_ = std::thread([this]() {
-    while (true) {
-      decoder_wrapper_->process_packet(packet_queue_.pop());
-    }
-  });
-
   if (launch_hw_) {
     hw_interface_wrapper_->hw_interface()->register_packet_callback(std::bind(
       &ContinentalARS548RosWrapper::receive_packet_callback, this, std::placeholders::_1));
@@ -155,7 +146,7 @@ void ContinentalARS548RosWrapper::receive_packets_callback(
     nebula_packet_ptr->stamp = packet.stamp;
     nebula_packet_ptr->data = std::move(packet.data);
 
-    packet_queue_.push(std::move(nebula_packet_ptr));
+    decoder_wrapper_->process_packet(std::move(nebula_packet_ptr));
   }
 }
 
@@ -166,9 +157,7 @@ void ContinentalARS548RosWrapper::receive_packet_callback(
     return;
   }
 
-  if (!packet_queue_.try_push(std::move(msg_ptr))) {
-    RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped");
-  }
+  decoder_wrapper_->process_packet(std::move(msg_ptr));
 }
 
 Status ContinentalARS548RosWrapper::get_status()

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -374,7 +374,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change(
     return rcl_interfaces::build<SetParametersResult>().successful(true).reason("");
   }
 
-  if (return_mode.empty()) {
+  if (!return_mode.empty()) {
     new_cfg.return_mode =
       nebula::drivers::return_mode_from_string_hesai(return_mode, sensor_cfg_ptr_->sensor_model);
   }

nebula_ros/src/velodyne/hw_monitor_wrapper.cpp

@@ -17,7 +17,8 @@ VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper(
   const std::shared_ptr<nebula::drivers::VelodyneHwInterface> & hw_interface,
   std::shared_ptr<const nebula::drivers::VelodyneSensorConfiguration> & 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),
@@ -30,18 +31,19 @@ VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper(
   std::cout << "Get model name and serial." << std::endl;
   auto str = hw_interface_->get_snapshot();
   if (!str.has_value()) return;
-  current_snapshot_time_.reset(new rclcpp::Time(parent_node_->now()));
-  current_snapshot_tree_ =
+
+  auto snapshot_tree =
     std::make_shared<boost::property_tree::ptree>(hw_interface_->parse_json(str.value()));
   current_diag_tree_ =
-    std::make_shared<boost::property_tree::ptree>(current_snapshot_tree_->get_child("diag"));
+    std::make_shared<boost::property_tree::ptree>(snapshot_tree->get_child("diag"));
   current_status_tree_ =
-    std::make_shared<boost::property_tree::ptree>(current_snapshot_tree_->get_child("status"));
-  current_snapshot_.reset(new std::string(str.value()));
+    std::make_shared<boost::property_tree::ptree>(snapshot_tree->get_child("status"));
 
   try {
-    info_model_ = get_ptree_value(current_snapshot_tree_, mtx_snapshot_, key_info_model);
-    info_serial_ = get_ptree_value(current_snapshot_tree_, mtx_snapshot_, key_info_serial);
+    // get_ptree_value requires a mutex but we are only accessing a local variable
+    std::mutex dummy_mtx;
+    info_model_ = get_ptree_value(snapshot_tree, dummy_mtx, key_info_model);
+    info_serial_ = get_ptree_value(snapshot_tree, dummy_mtx, key_info_serial);
     RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_);
     RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_);
   } catch (boost::bad_lexical_cast & ex) {
@@ -62,10 +64,6 @@ void VelodyneHwMonitorWrapper::initialize_velodyne_diagnostics()
   RCLCPP_INFO_STREAM(logger_, "Hardware ID: " << hardware_id);
 
   if (show_advanced_diagnostics_) {
-    diagnostics_updater_.add(
-      "velodyne_snapshot-" + sensor_configuration_->frame_id, this,
-      &VelodyneHwMonitorWrapper::velodyne_check_snapshot);
-
     diagnostics_updater_.add(
       "velodyne_volt_temp_top_hv-" + sensor_configuration_->frame_id, this,
       &VelodyneHwMonitorWrapper::velodyne_check_top_hv);
@@ -164,55 +162,7 @@ void VelodyneHwMonitorWrapper::initialize_velodyne_diagnostics()
   diagnostics_updater_.add(
     "velodyne_voltage", this, &VelodyneHwMonitorWrapper::velodyne_check_voltage);
 
-  {
-    std::lock_guard lock(mtx_snapshot_);
-    current_snapshot_.reset(new std::string(""));
-    current_snapshot_time_.reset(new rclcpp::Time(parent_node_->now()));
-  }
-
-  current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE;
-
-  auto on_timer_snapshot = [this] { on_velodyne_snapshot_timer(); };
-  diagnostics_snapshot_timer_ = parent_node_->create_wall_timer(
-    std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot));
-
-  auto on_timer_update = [this] {
-    auto now = parent_node_->now();
-    double dif;
-    {
-      std::lock_guard lock(mtx_snapshot_);
-      dif = (now - *current_snapshot_time_).seconds();
-    }
-    if (diag_span_ * 2.0 < dif * 1000) {
-      current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE;
-      RCLCPP_DEBUG_STREAM(logger_, "STALE");
-    } else {
-      current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK;
-      RCLCPP_DEBUG_STREAM(logger_, "OK");
-    }
-    diagnostics_updater_.force_update();
-  };
-  diagnostics_update_timer_ =
-    parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update));
-}
-
-void VelodyneHwMonitorWrapper::on_velodyne_snapshot_timer()
-{
-  auto str = hw_interface_->get_snapshot();
-  if (!str.has_value()) return;
-  auto ptree = hw_interface_->parse_json(str.value());
-
-  {
-    std::lock_guard lock(mtx_snapshot_);
-
-    current_snapshot_time_.reset(new rclcpp::Time(parent_node_->now()));
-    current_snapshot_tree_ = std::make_shared<boost::property_tree::ptree>(ptree);
-    current_diag_tree_ =
-      std::make_shared<boost::property_tree::ptree>(current_snapshot_tree_->get_child("diag"));
-    current_status_tree_ =
-      std::make_shared<boost::property_tree::ptree>(current_snapshot_tree_->get_child("status"));
-    current_snapshot_.reset(new std::string(str.value()));
-  }
+  diagnostics_updater_.setPeriod(1.0);
 }
 
 void VelodyneHwMonitorWrapper::on_velodyne_diagnostics_timer()
@@ -1238,14 +1188,6 @@ void VelodyneHwMonitorWrapper::velodyne_check_laser_state(
   }
 }
 
-void VelodyneHwMonitorWrapper::velodyne_check_snapshot(
-  diagnostic_updater::DiagnosticStatusWrapper & diagnostics)
-{
-  uint8_t level = current_diag_status_;
-  diagnostics.add("sensor", sensor_configuration_->frame_id);
-  diagnostics.summary(level, *current_snapshot_);
-}
-
 void VelodyneHwMonitorWrapper::velodyne_check_status(
   diagnostic_updater::DiagnosticStatusWrapper & diagnostics)
 {

nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp

@@ -18,11 +18,7 @@ namespace nebula::ros
 VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options)
 : rclcpp::Node("velodyne_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_()
+  sensor_cfg_ptr_(nullptr)
 {
   setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 
@@ -56,12 +52,6 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options)
 
   RCLCPP_DEBUG(get_logger(), "Starting stream");
 
-  decoder_thread_ = std::thread([this]() {
-    while (true) {
-      decoder_wrapper_->process_cloud_packet(packet_queue_.pop());
-    }
-  });
-
   if (launch_hw_) {
     hw_interface_wrapper_->hw_interface()->register_scan_callback(
       std::bind(&VelodyneRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1));
@@ -172,7 +162,7 @@ void VelodyneRosWrapper::receive_scan_message_callback(
     nebula_pkt_ptr->stamp = pkt.stamp;
     std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data));
 
-    packet_queue_.push(std::move(nebula_pkt_ptr));
+    decoder_wrapper_->process_cloud_packet(std::move(nebula_pkt_ptr));
   }
 }
 
@@ -263,9 +253,7 @@ void VelodyneRosWrapper::receive_cloud_packet_callback(std::vector<uint8_t> & pa
   msg_ptr->stamp.nanosec = static_cast<int>(timestamp_ns % 1'000'000'000);
   msg_ptr->data.swap(packet);
 
-  if (!packet_queue_.try_push(std::move(msg_ptr))) {
-    RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped");
-  }
+  decoder_wrapper_->process_cloud_packet(std::move(msg_ptr));
 }
 
 RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneRosWrapper)