From 8c8ccfde114bf227be25d9efeaca6c9d6c4b922f Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Wed, 8 Nov 2023 11:04:48 -0800 Subject: [PATCH] SW-5617: resolve issue with crash on exist in ros-1 driver (#243) * Gracefully stop the driver on exit (or node kill) * Update version and CHANGELOG --- CHANGELOG.rst | 1 + launch/driver.launch | 4 +--- launch/record.launch | 4 +--- launch/replay.launch | 4 +--- launch/sensor.launch | 4 +--- launch/sensor_mtp.launch | 1 - package.xml | 2 +- src/os_driver_nodelet.cpp | 6 ++++-- src/os_sensor_nodelet.cpp | 21 ++++++++++++++------- src/os_sensor_nodelet.h | 8 +++++--- 10 files changed, 29 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 21794558..677f7e71 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +11,7 @@ ouster_ros(1) to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode is used. * fix: destagger columns timestamp when generating destaggered point clouds. +* fix: gracefully stop the driver when shutdown is requested. ouster_ros v0.10.0 diff --git a/launch/driver.launch b/launch/driver.launch index 7ecc5a82..0f034c11 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -57,9 +57,7 @@ + output="screen" required="true" args="manager"/> diff --git a/launch/record.launch b/launch/record.launch index cfa39c3e..04ba0213 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -60,9 +60,7 @@ + output="screen" required="true" args="manager"/> diff --git a/launch/replay.launch b/launch/replay.launch index 97d58e9e..ca73395c 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -40,9 +40,7 @@ + output="screen" required="true" args="manager"/> diff --git a/launch/sensor.launch b/launch/sensor.launch index 942d23af..ecc950e5 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -65,9 +65,7 @@ + output="screen" required="true" args="manager"/> diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 5f481009..9784a281 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -70,7 +70,6 @@ diff --git a/package.xml b/package.xml index 32ed19b4..b0d260f8 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.9.2 + 0.9.3 Ouster ROS driver ouster developers BSD diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 8c602cee..0d56db5a 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -32,10 +32,12 @@ namespace ouster_ros { class OusterDriver : public OusterSensor { public: OusterDriver() : tf_bcast(getName()) {} + ~OusterDriver() override { + NODELET_DEBUG("OusterDriver::~OusterDriver() called"); + halt(); + } private: - // virtual void onInit() override { - // } virtual void on_metadata_updated(const sensor::sensor_info& info) override { OusterSensor::on_metadata_updated(info); diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index b7233bc9..e95d8b35 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -29,6 +29,16 @@ using namespace std::string_literals; namespace ouster_ros { +OusterSensor::~OusterSensor() { + NODELET_DEBUG("OusterDriver::~OusterSensor() called"); + halt(); +} + +void OusterSensor::halt() { + stop_packet_processing_threads(); + stop_sensor_connection_thread(); +} + void OusterSensor::onInit() { sensor_hostname = get_sensor_hostname(); sensor::sensor_config config = parse_config_from_ros_parameters(); @@ -547,24 +557,21 @@ void OusterSensor::stop_sensor_connection_thread() { } void OusterSensor::start_packet_processing_threads() { - imu_packets_skip = false; imu_packets_processing_thread_active = true; imu_packets_processing_thread = std::make_unique([this]() { - auto read_packet = [this](const uint8_t* buffer) { - if (!imu_packets_skip) on_imu_packet_msg(buffer); - }; while (imu_packets_processing_thread_active) { - imu_packets->read(read_packet); + imu_packets->read([this](const uint8_t* buffer) { + on_imu_packet_msg(buffer); + }); } NODELET_DEBUG("imu_packets_processing_thread done."); }); - lidar_packets_skip = false; lidar_packets_processing_thread_active = true; lidar_packets_processing_thread = std::make_unique([this]() { while (lidar_packets_processing_thread_active) { lidar_packets->read([this](const uint8_t* buffer) { - if (!lidar_packets_skip) on_lidar_packet_msg(buffer); + on_lidar_packet_msg(buffer); }); } diff --git a/src/os_sensor_nodelet.h b/src/os_sensor_nodelet.h index 6e636ca8..fcaab051 100644 --- a/src/os_sensor_nodelet.h +++ b/src/os_sensor_nodelet.h @@ -29,6 +29,9 @@ namespace sensor = ouster::sensor; namespace ouster_ros { class OusterSensor : public OusterSensorNodeletBase { + public: + ~OusterSensor() override; + private: virtual void onInit() override; @@ -43,6 +46,8 @@ class OusterSensor : public OusterSensorNodeletBase { virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet); + void halt(); + private: std::string get_sensor_hostname(); @@ -75,7 +80,6 @@ class OusterSensor : public OusterSensorNodeletBase { std::string load_config_file(const std::string& config_file); - private: // fill in values that could not be parsed from metadata void populate_metadata_defaults(sensor::sensor_info& info, sensor::lidar_mode specified_lidar_mode); @@ -129,11 +133,9 @@ class OusterSensor : public OusterSensorNodeletBase { std::unique_ptr sensor_connection_thread; std::atomic imu_packets_processing_thread_active = {false}; - std::atomic imu_packets_skip; std::unique_ptr imu_packets_processing_thread; std::atomic lidar_packets_processing_thread_active = {false}; - std::atomic lidar_packets_skip; std::unique_ptr lidar_packets_processing_thread; bool force_sensor_reinit = false;