From f8e5fd00d4543f46303674080ae02eb01db47ce5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER 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 --- .../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 current_status; std::unique_ptr current_monitor; std::unique_ptr 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 @@ - + 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(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 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>( - 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>( + 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>( - 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>( - 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 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 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file