SW-5617: resolve issue with crash on exist in ros-1 driver

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

launch/driver.launch

@@ -57,9 +57,7 @@
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
-      output="screen" required="true"
-      launch-prefix="bash -c 'sleep 2; $0 $@' "
-      args="manager"/>
+      output="screen" required="true" args="manager"/>
   </group>
 
   <group ns="$(arg ouster_ns)">

launch/record.launch

@@ -60,9 +60,7 @@
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
-      output="screen" required="true"
-      launch-prefix="bash -c 'sleep 2; $0 $@' "
-      args="manager"/>
+      output="screen" required="true" args="manager"/>
   </group>
 
   <group ns="$(arg ouster_ns)">

launch/replay.launch

@@ -40,9 +40,7 @@
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
-      output="screen" required="true"
-      launch-prefix="bash -c 'sleep 2; $0 $@' "
-      args="manager"/>
+      output="screen" required="true" args="manager"/>
   </group>
 
   <arg name="_use_metadata_file" value="$(eval not (metadata == ''))"/>

launch/sensor.launch

@@ -65,9 +65,7 @@
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
-      output="screen" required="true"
-      launch-prefix="bash -c 'sleep 2; $0 $@' "
-      args="manager"/>
+      output="screen" required="true" args="manager"/>
   </group>
 
   <group ns="$(arg ouster_ns)">

launch/sensor_mtp.launch

@@ -70,7 +70,6 @@
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true"
-      launch-prefix="bash -c 'sleep 2; $0 $@' "
       args="manager"/>
   </group>
 

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.9.2</version>
+  <version>0.9.3</version>
   <description>Ouster ROS driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

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);

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<std::thread>([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<std::thread>([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);
             });
         }
 

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<std::thread> sensor_connection_thread;
 
     std::atomic<bool> imu_packets_processing_thread_active = {false};
-    std::atomic<bool> imu_packets_skip;
     std::unique_ptr<std::thread> imu_packets_processing_thread;
 
     std::atomic<bool> lidar_packets_processing_thread_active = {false};
-    std::atomic<bool> lidar_packets_skip;
     std::unique_ptr<std::thread> lidar_packets_processing_thread;
 
     bool force_sensor_reinit = false;