ROS-119: ouster-ros driver automatic reconnection (#361)

* Implement sensor reconnection behavior
* Implement restart for os_image nodelet
* Create all services/publishers/subscribers on startup
* Check proc_mask before enabling a topic
* Remove uneeded relay
* Properly handle sensor configuration outside of driver
* Persist config only on startup and only auto_udp if startup config didn't set an address
* Add reconnection dormant period and max attempts

CHANGELOG.rst

@@ -19,9 +19,11 @@ Changelog
   explicitly by turning on the ``BUILD_PCAP`` cmake option and having ``libpcap-dev`` installed.
 * [BREAKING] Added new launch files args ``azimuth_window_start`` and ``azimuth_window_end`` to
   allow users to set LIDAR FOV on startup. The new options will reset the current azimuth window
-  to the default
+  to the default [0, 360] azimuth if not configured.
 * Added a new launch ``persist_config`` option to request the sensor persist the current config
 * Added a new ``loop`` option to the ``replay.launch`` file.
+* Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg
+  documentation and the associated params to enable.
 
 
 ouster_ros v0.10.0

CMakeLists.txt

@@ -75,13 +75,20 @@ include_directories(${_ouster_ros_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
 # use only MPL-licensed parts of eigen
 add_definitions(-DEIGEN_MPL2_ONLY)
 
+set(NODELET_SRC
+  src/os_sensor_nodelet_base.cpp
+  src/os_sensor_nodelet.cpp
+  src/os_replay_nodelet.cpp
+  src/os_cloud_nodelet.cpp
+  src/os_image_nodelet.cpp
+  src/os_driver_nodelet.cpp
+)
+
 set(OUSTER_TARGET_LINKS ouster_client)
+
 if (BUILD_PCAP)
   list(APPEND OUSTER_TARGET_LINKS ouster_pcap)
-  # Include os_pcap_nodelet.cpp only if BUILD_PCAP is defined
-  set(PCAP_NODELET_SRC src/os_pcap_nodelet.cpp)
-else()
-  set(PCAP_NODELET_SRC "")
+  list(APPEND NODELET_SRC src/os_pcap_nodelet.cpp)
 endif()
 
 add_library(ouster_ros src/os_ros.cpp)
@@ -91,14 +98,7 @@ target_link_libraries(ouster_ros
 add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp)
 
 # ==== Executables ====
-add_library(${PROJECT_NAME}_nodelets
-  src/os_sensor_nodelet_base.cpp
-  src/os_sensor_nodelet.cpp
-  src/os_replay_nodelet.cpp
-  ${PCAP_NODELET_SRC} # Include os_pcap_nodelet.cpp only if BUILD_PCAP is defined
-  src/os_cloud_nodelet.cpp
-  src/os_image_nodelet.cpp
-  src/os_driver_nodelet.cpp)
+add_library(${PROJECT_NAME}_nodelets ${NODELET_SRC})
 target_link_libraries(${PROJECT_NAME}_nodelets ouster_ros ${catkin_LIBRARIES})
 add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp)
 

include/ouster_ros/os_sensor_nodelet_base.h

@@ -22,7 +22,7 @@ class OusterSensorNodeletBase : public nodelet::Nodelet {
 
     void create_get_metadata_service();
 
-    void create_metadata_publisher();
+    void create_metadata_pub();
 
     void publish_metadata();
 

launch/common.launch

@@ -67,6 +67,7 @@
     <node pkg="nodelet" type="nodelet" name="img_node"
       output="screen" required="true"
       args="load ouster_ros/OusterImage os_nodelet_mgr $(arg _no_bond)">
+      <param name="~/proc_mask" value="$(arg proc_mask)"/>
     </node>
   </group>
 
@@ -77,12 +78,4 @@
     launch-prefix="bash -c 'sleep 5; $0 $@' "
     args="-d $(arg rviz_config)"/>
 
-  <!-- for compatibility with < 2.0 rosbags, enable the following block -->
-  <!--
-  <node pkg="topic_tools" name="relay_lidar" type="relay"
-    args="/os_node/lidar_packets /$(arg ouster_ns)/lidar_packets"/>
-  <node pkg="topic_tools" name="relay_imu" type="relay"
-    args="/os_node/imu_packets /$(arg ouster_ns)/imu_packets"/>
-  -->
-
 </launch>

launch/driver.launch

@@ -73,6 +73,15 @@
   <arg name="persist_config" default="false"
     doc="request the sensor to persist settings"/>
 
+  <arg name="attempt_reconnect" default="false"
+    doc="attempting to reconnect to the sensor after connection loss or
+    sensor powered down"/>
+  <arg name="dormant_period_between_reconnects" default="1.0"
+    doc="wait time in seconds between reconnection attempts"/>
+  <arg name="max_failed_reconnect_attempts" default="2147483647"
+    doc="maximum number of attempts trying to communicate with the sensor.
+         Counter resets upon successful connection"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -103,6 +112,11 @@
       <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
       <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
       <param name="~/persist_config" value="$(arg persist_config)"/>
+      <param name="~/attempt_reconnect" value="$(arg attempt_reconnect)"/>
+      <param name="~/dormant_period_between_reconnects"
+             value="$(arg dormant_period_between_reconnects)"/>
+      <param name="~/max_failed_reconnect_attempts"
+             value="$(arg max_failed_reconnect_attempts)"/>
     </node>
   </group>
 

launch/record.launch

@@ -76,6 +76,15 @@
   <arg name="persist_config" default="false"
     doc="request the sensor to persist settings"/>
 
+  <arg name="attempt_reconnect" default="false"
+    doc="attempting to reconnect to the sensor after connection loss or
+    sensor powered down"/>
+  <arg name="dormant_period_between_reconnects" default="1.0"
+    doc="wait time in seconds between reconnection attempts"/>
+  <arg name="max_failed_reconnect_attempts" default="2147483647"
+    doc="maximum number of attempts trying to communicate with the sensor.
+         Counter resets upon successful connection"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -97,6 +106,11 @@
       <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
       <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
       <param name="~/persist_config" value="$(arg persist_config)"/>
+      <param name="~/attempt_reconnect" value="$(arg attempt_reconnect)"/>
+      <param name="~/dormant_period_between_reconnects"
+             value="$(arg dormant_period_between_reconnects)"/>
+      <param name="~/max_failed_reconnect_attempts"
+             value="$(arg max_failed_reconnect_attempts)"/>
     </node>
   </group>
 

launch/sensor.launch

@@ -81,6 +81,15 @@
   <arg name="persist_config" default="false"
     doc="request the sensor to persist settings"/>
 
+  <arg name="attempt_reconnect" default="false"
+    doc="attempting to reconnect to the sensor after connection loss or
+    sensor powered down"/>
+  <arg name="dormant_period_between_reconnects" default="1.0"
+    doc="wait time in seconds between reconnection attempts"/>
+  <arg name="max_failed_reconnect_attempts" default="2147483647"
+    doc="maximum number of attempts trying to communicate with the sensor.
+         Counter resets upon successful connection"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -101,6 +110,11 @@
       <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
       <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
       <param name="~/persist_config" value="$(arg persist_config)"/>
+      <param name="~/attempt_reconnect" value="$(arg attempt_reconnect)"/>
+      <param name="~/dormant_period_between_reconnects"
+             value="$(arg dormant_period_between_reconnects)"/>
+      <param name="~/max_failed_reconnect_attempts"
+             value="$(arg max_failed_reconnect_attempts)"/>
     </node>
   </group>
 

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.12.5</version>
+  <version>0.12.6</version>
   <description>Ouster ROS driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

src/lidar_packet_handler.h

@@ -18,6 +18,7 @@
 
 #include "lock_free_ring_buffer.h"
 #include <thread>
+#include <chrono>
 
 namespace {
 
@@ -125,7 +126,13 @@ class LidarPacketHandler {
 
     LidarPacketHandler(const LidarPacketHandler&) = delete;
     LidarPacketHandler& operator=(const LidarPacketHandler&) = delete;
-    ~LidarPacketHandler() = default;
+    ~LidarPacketHandler() {
+        NODELET_DEBUG("LidarPacketHandler::~LidarPacketHandler()");
+        if (lidar_scans_processing_thread->joinable()) {
+            lidar_scans_processing_active = false;
+            lidar_scans_processing_thread->join();
+        }
+    }
 
     void register_lidar_scan_handler(LidarScanProcessor handler) {
         lidar_scan_handlers.push_back(handler);
@@ -151,11 +158,17 @@ class LidarPacketHandler {
 
     void process_scans() {
 
+        using namespace std::chrono;
+
         {
             std::unique_lock<std::mutex> index_lock(ring_buffer_mutex);
-            ring_buffer_has_elements.wait(index_lock, [this] {
+            ring_buffer_has_elements.wait_for(index_lock, 1s, [this] {
                 return !ring_buffer.empty();
             });
+
+            if (ring_buffer.empty()) {
+                return;
+            }
         }
 
         std::unique_lock<std::mutex> lock(*mutexes[ring_buffer.read_head()]);

src/os_cloud_nodelet.cpp

@@ -39,6 +39,18 @@ class OusterCloud : public nodelet::Nodelet {
 
    private:
     virtual void onInit() override {
+        auto& pnh = getPrivateNodeHandle();
+        auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
+        auto tokens = impl::parse_tokens(proc_mask, '|');
+        if (impl::check_token(tokens, "IMU"))
+            create_imu_pub_sub();
+        if (impl::check_token(tokens, "PCL"))
+            create_point_cloud_pubs();
+        if (impl::check_token(tokens, "SCAN"))
+            create_laser_scan_pubs();
+        if (impl::check_token(tokens, "PCL") ||
+            impl::check_token(tokens, "SCAN"))
+            create_lidar_packets_sub();
         create_metadata_subscriber();
         NODELET_INFO("OusterCloud: nodelet created!");
     }
@@ -49,15 +61,12 @@ class OusterCloud : public nodelet::Nodelet {
     }
 
     void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) {
-        // TODO: handle sensor reconfigurtion
         NODELET_INFO("OusterCloud: retrieved new sensor metadata!");
-
         auto info = sensor::parse_metadata(metadata_msg->data);
 
-        tf_bcast.parse_parameters(getPrivateNodeHandle());
-
-        auto dynamic_transforms =
-            getPrivateNodeHandle().param("dynamic_transforms_broadcast", false);
+        auto pnh = getPrivateNodeHandle();
+        tf_bcast.parse_parameters(pnh);
+        auto dynamic_transforms = pnh.param("dynamic_transforms_broadcast", false);
         auto dynamic_transforms_rate = getPrivateNodeHandle().param(
             "dynamic_transforms_broadcast_rate", 1.0);
         if (dynamic_transforms && dynamic_transforms_rate < 1.0) {
@@ -72,53 +81,74 @@ class OusterCloud : public nodelet::Nodelet {
             tf_bcast.broadcast_transforms(info);
         } else {
             NODELET_INFO_STREAM(
-                "OusterCloud: dynamic transforms broadcast enabled wit "
+                "OusterCloud: dynamic transforms broadcast enabled with "
                 "broadcast rate of: "
                 << dynamic_transforms_rate << " Hz");
+            timer_.stop();
             timer_ = getNodeHandle().createTimer(
                 ros::Duration(1.0 / dynamic_transforms_rate),
                 [this, info](const ros::TimerEvent&) {
                     tf_bcast.broadcast_transforms(info, last_msg_ts);
                 });
         }
 
-        create_publishers_subscribers(info);
+        create_handlers(info);
+    }
+
+    void create_imu_pub_sub() {
+        imu_pub = getNodeHandle().advertise<sensor_msgs::Imu>("imu", 100);
+        imu_packet_sub = getNodeHandle().subscribe<PacketMsg>(
+            "imu_packets", 100, [this](const PacketMsg::ConstPtr msg) {
+                if (imu_packet_handler) {
+                    auto imu_msg = imu_packet_handler(msg->buf.data());
+                    if (imu_msg.header.stamp > last_msg_ts)
+                        last_msg_ts = imu_msg.header.stamp;
+                    imu_pub.publish(imu_msg);
+                }
+            });
+    }
+
+    void create_point_cloud_pubs() {
+        // NOTE: always create the 2nd topic
+        lidar_pubs.resize(2);
+        for (int i = 0; i < 2; ++i) {
+            lidar_pubs[i] = getNodeHandle().advertise<sensor_msgs::PointCloud2>(
+                topic_for_return("points", i), 10);
+        }
+    }
+
+    void create_laser_scan_pubs() {
+        // NOTE: always create the 2nd topic
+        scan_pubs.resize(2);
+        for (int i = 0; i < 2; ++i) {
+            scan_pubs[i] = getNodeHandle().advertise<sensor_msgs::LaserScan>(
+                topic_for_return("scan", i), 10);
+        }
+    }
+
+    void create_lidar_packets_sub() {
+        lidar_packet_sub = getNodeHandle().subscribe<PacketMsg>(
+        "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
+            if (lidar_packet_handler) lidar_packet_handler(msg->buf.data());
+        });
     }
 
-    void create_publishers_subscribers(const sensor::sensor_info& info) {
+    void create_handlers(const sensor::sensor_info& info) {
         auto& pnh = getPrivateNodeHandle();
         auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
         auto tokens = impl::parse_tokens(proc_mask, '|');
 
         auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
         double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
 
-        auto& nh = getNodeHandle();
-
         if (impl::check_token(tokens, "IMU")) {
-            imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 100);
             imu_packet_handler = ImuPacketHandler::create_handler(
                 info, tf_bcast.imu_frame_id(), timestamp_mode,
                 static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
-            imu_packet_sub = nh.subscribe<PacketMsg>(
-                "imu_packets", 100, [this](const PacketMsg::ConstPtr msg) {
-                    auto imu_msg = imu_packet_handler(msg->buf.data());
-                    if (imu_msg.header.stamp > last_msg_ts)
-                        last_msg_ts = imu_msg.header.stamp;
-                    imu_pub.publish(imu_msg);
-                });
         }
 
-        int num_returns = get_n_returns(info);
-
         std::vector<LidarScanProcessor> processors;
         if (impl::check_token(tokens, "PCL")) {
-            lidar_pubs.resize(num_returns);
-            for (int i = 0; i < num_returns; ++i) {
-                lidar_pubs[i] = nh.advertise<sensor_msgs::PointCloud2>(
-                    topic_for_return("points", i), 10);
-            }
-
             auto point_type = pnh.param("point_type", std::string{"original"});
             processors.push_back(
                 PointCloudProcessorFactory::create_point_cloud_processor(point_type,
@@ -145,12 +175,6 @@ class OusterCloud : public nodelet::Nodelet {
         }
 
         if (impl::check_token(tokens, "SCAN")) {
-            scan_pubs.resize(num_returns);
-            for (int i = 0; i < num_returns; ++i) {
-                scan_pubs[i] = nh.advertise<sensor_msgs::LaserScan>(
-                    topic_for_return("scan", i), 10);
-            }
-
             // TODO: avoid this duplication in os_cloud_node
             int beams_count = static_cast<int>(get_beams_count(info));
             int scan_ring = pnh.param("scan_ring", 0);
@@ -177,11 +201,8 @@ class OusterCloud : public nodelet::Nodelet {
             lidar_packet_handler = LidarPacketHandler::create_handler(
                 info, processors, timestamp_mode,
                 static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
-            lidar_packet_sub = nh.subscribe<PacketMsg>(
-                "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
-                    lidar_packet_handler(msg->buf.data());
-                });
         }
+
     }
 
    private: