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

Author: knzo25

.codecov.yml

@@ -1,3 +1,43 @@
+coverage:
+  status:
+    project:
+      default:
+        target: auto
+        # allow slight decreases in coverage compared to the base ref
+        threshold: 1%
+
 ignore:
-  - build
-  - nebula_examples
+  - ^build.* # for generated message wrappers
+  - ^nebula_examples.* # not safety-related
+
+comment:
+  layout: header, diff, flags, components
+
+component_management:
+  default_rules:
+    statuses:
+      - type: project # inherit project rules for coverage statuses
+  individual_components:
+    - component_id: common
+      name: Common
+      paths:
+        - "!**hesai**"
+        - "!**velodyne**"
+        - "!**continental**"
+        - "!**robosense**"
+    - component_id: hesai
+      name: Hesai
+      paths:
+        - "**hesai**"
+    - component_id: velodyne
+      name: Velodyne
+      paths:
+        - "**velodyne**"
+    - component_id: continental
+      name: Continental
+      paths:
+        - "**continental**"
+    - component_id: robosense
+      name: Robosense
+      paths:
+        - "**robosense**"

.pre-commit-config.yaml

@@ -53,7 +53,7 @@ repos:
         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/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/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/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/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)
 {