From f8e5fd00d4543f46303674080ae02eb01db47ce5 Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Tue, 2 Apr 2024 21:29:53 +0900
Subject: [PATCH 1/2] fix(hesai_hw_monitor): merge timers to fix race condition
accessing TCP driver
Signed-off-by: Max SCHMELLER <max.schmeller@tier4.jp>
---
.../hesai/hesai_hw_monitor_ros_wrapper.hpp | 3 +-
nebula_ros/launch/hesai_launch_all_hw.xml | 2 +-
.../hesai/hesai_hw_monitor_ros_wrapper.cpp | 38 ++++++++++---------
3 files changed, 22 insertions(+), 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
index 857f638cd..c23b76c55 100644
--- 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
index b26193414..f4a1cd41c 100644
--- 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/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp
index 239f6b8b0..85476075e 100644
--- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp
+++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp
@@ -63,6 +63,7 @@ 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{};
@@ -70,8 +71,8 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o
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(
From ed288ee9ac45e0e139ab6d53d80274c8a3378e3c Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Tue, 2 Apr 2024 21:43:13 +0900
Subject: [PATCH 2/2] fix(hesai_launch_component.launch.xml): launch HW monitor
in the default component launch config
---
.../launch/hesai_launch_component.launch.xml | 25 +++++++++++++++++++
1 file changed, 25 insertions(+)
diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml
index 42e96a25e..75c7414da 100644
--- 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>
\ No newline at end of file