fix(hesai): gracefully stop receiver thread on shutdown to prevent SE…

…GFAULT Signed-off-by: Max SCHMELLER <[email protected]>
tier4 · Nov 21, 2024 · 5fd3142 · 5fd3142
1 parent de0ded4
commit 5fd3142
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
@@ -197,7 +197,8 @@ void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector<uint8_t> &
 
 Status HesaiHwInterface::SensorInterfaceStop()
 {
-  return Status::ERROR_1;
+  udp_socket_.unsubscribe();
+  return Status::OK;
 }
 
 Status HesaiHwInterface::GetSensorConfiguration(

diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp
@@ -51,7 +51,11 @@ class HesaiRosWrapper final : public rclcpp::Node
 
 public:
   explicit HesaiRosWrapper(const rclcpp::NodeOptions & options);
-  ~HesaiRosWrapper() noexcept override = default;
+  ~HesaiRosWrapper() noexcept override
+  {
+    if (!hw_interface_wrapper_) return;
+    hw_interface_wrapper_->hw_interface()->SensorInterfaceStop();
+  };
 
   /// @brief Get current status of this driver
   /// @return Current status