fix(hesai_hw_monitor): fix 2s drops issue - again (#135)

* fix(hesai_hw_monitor): merge timers to fix race condition accessing TCP driver Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai_launch_component.launch.xml): launch HW monitor in the default component launch config --------- Signed-off-by: Max SCHMELLER <[email protected]> Co-authored-by: Max SCHMELLER <[email protected]>
tier4 · Apr 3, 2024 · eb5e1b7 · eb5e1b7
1 parent 1ae3f75
commit eb5e1b7
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 21 deletions.
diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp
@@ -109,8 +109,7 @@ class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapp
 
   rclcpp::TimerBase::SharedPtr diagnostics_update_timer_;
   rclcpp::TimerBase::SharedPtr diagnostics_update_monitor_timer_;
-  rclcpp::TimerBase::SharedPtr diagnostics_status_timer_;
-  rclcpp::TimerBase::SharedPtr diagnostics_lidar_monitor_timer_;
+  rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_;
   std::unique_ptr<HesaiLidarStatus> current_status;
   std::unique_ptr<HesaiLidarMonitor> current_monitor;
   std::unique_ptr<HesaiConfig> current_config;

diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="launch_hw" default="True" />
+    <arg name="launch_hw" default="true" />
     <arg name="sensor_model" description="Pandar64|Pandar40P|PandarXT32|PandarXT32M|PandarAT128|PandarQT64|PandarQT128|Pandar128E4X"/>
     <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
     <arg name="frame_id"  default="hesai"/>

diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml
@@ -85,6 +85,31 @@
                 <extra_arg name="use_intra_process_comms" value="true" />
             </composable_node>
         </load_composable_node>
+
+        <load_composable_node target="PandarContainer">
+            <composable_node pkg="nebula_ros" 
+                             plugin="HesaiHwMonitorRosWrapper"
+                             name="PandarMon" 
+                             namespace="/">
+                <param name="sensor_model" value="$(var sensor_model)"/>
+                <param name="return_mode" value="$(var return_mode)"/>
+                <param name="frame_id" value="$(var frame_id)"/>
+                <param name="scan_phase" value="$(var scan_phase)"/>
+                <param name="sensor_ip" value="$(var sensor_ip)"/>
+                <param name="frame_id" value="$(var frame_id)"/>
+                <param name="host_ip" value="$(var host_ip)"/>
+                <param name="data_port" value="$(var data_port)"/>
+                <param name="gnss_port" value="$(var gnss_port)"/>
+                <param name="packet_mtu_size" value="$(var packet_mtu_size)"/>
+                <param name="rotation_speed" value="$(var rotation_speed)"/>
+                <param name="cloud_min_angle" value="$(var cloud_min_angle)"/>
+                <param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
+                <param name="diag_span" value="$(var diag_span)"/>
+                <param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
+                <param name="delay_monitor_ms" value="$(var delay_monitor_ms)"/>
+                <extra_arg name="use_intra_process_comms" value="true" />
+            </composable_node>
+        </load_composable_node>
     </group>
 
 </launch>
diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp
@@ -63,15 +63,16 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o
     std::static_pointer_cast<drivers::SensorConfigurationBase>(sensor_cfg_ptr));
   while(hw_interface_.InitializeTcpDriver() == Status::ERROR_1)
   {
+    RCLCPP_WARN(this->get_logger(), "Could not initialize TCP driver, retrying in 8s...");
     std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000
   }
   std::vector<std::thread> thread_pool{};
   thread_pool.emplace_back([this] {
     auto result = hw_interface_.GetInventory();
     current_inventory.reset(new HesaiInventory(result));
     current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now()));
-    std::cout << "HesaiInventory" << std::endl;
-    std::cout << result << std::endl;
+    RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory");
+    RCLCPP_INFO_STREAM(this->get_logger(), result);
     info_model = result.get_str_model();
     info_serial = std::string(result.sn.begin(), result.sn.end());
     hw_interface_.SetTargetModel(result.model);
@@ -303,29 +304,25 @@ void HesaiHwMonitorRosWrapper::InitializeHesaiDiagnostics()
   current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE;
   current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE;
 
-  auto on_timer_status = [this] { OnHesaiStatusTimer(); };
-  diagnostics_status_timer_ = std::make_shared<rclcpp::GenericTimer<decltype(on_timer_status)>>(
-    this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_status),
+  auto fetch_diag_from_sensor = [this](){ 
+    OnHesaiStatusTimer(); 
+    if (hw_interface_.UseHttpGetLidarMonitor()) {
+      OnHesaiLidarMonitorTimerHttp();
+    } else {
+      OnHesaiLidarMonitorTimer();
+    }
+  };
+
+  fetch_diagnostics_timer_ = std::make_shared<rclcpp::GenericTimer<decltype(fetch_diag_from_sensor)>>(
+    this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor),
     this->get_node_base_interface()->get_context());
-  this->get_node_timers_interface()->add_timer(diagnostics_status_timer_, cbg_m_);
+  this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_);
 
   if (hw_interface_.UseHttpGetLidarMonitor()) {
     diagnostics_updater_.add(
       "hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp);
-    auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimerHttp(); };
-    diagnostics_lidar_monitor_timer_ =
-      std::make_shared<rclcpp::GenericTimer<decltype(on_timer_lidar_monitor)>>(
-        this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_lidar_monitor),
-        this->get_node_base_interface()->get_context());
-    this->get_node_timers_interface()->add_timer(diagnostics_lidar_monitor_timer_, cbg_m2_);
   } else {
     diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltage);
-    auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimer(); };
-    diagnostics_lidar_monitor_timer_ =
-      std::make_shared<rclcpp::GenericTimer<decltype(on_timer_lidar_monitor)>>(
-        this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_lidar_monitor),
-        this->get_node_base_interface()->get_context());
-    this->get_node_timers_interface()->add_timer(diagnostics_lidar_monitor_timer_, cbg_m2_);
   }
 
   auto on_timer_update = [this] {
@@ -425,6 +422,7 @@ void HesaiHwMonitorRosWrapper::OnHesaiStatusTimer()
         "HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"),
       error.what());
   }
+  RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl);
 }
 
 void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp()
@@ -448,6 +446,8 @@ void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp()
         "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"),
       error.what());
   }
+
+  RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END");
 }
 
 void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer()
@@ -469,6 +469,8 @@ void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer()
         "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"),
       error.what());
   }
+
+  RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END");
 }
 
 void HesaiHwMonitorRosWrapper::HesaiCheckStatus(