Handle cases of the lidar sensor becoming unpowered during operation

launch/record.launch

@@ -1,11 +1,12 @@
 <launch>
 
   <arg name="ouster_ns" default="ouster" doc="Override the default namespace of all ouster nodes"/>
-  <arg name="sensor_hostname" doc="hostname or IP in dotted decimal form of the sensor"/>
-  <arg name="udp_dest" default=" " doc="hostname or IP where the sensor will send data packets"/>
-  <arg name="lidar_port" default="0" doc="port to which the sensor should send lidar data"/>
-  <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
-  <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
+  <arg name="sensor_hostname" doc="The hostname or IP in dotted decimal form of the sensor"/>
+  <arg name="udp_dest" default=" " doc="The hostname or IP where the sensor will send data packets"/>
+  <arg name="retry_configuration" default="false" doc="True to re-attempt unsuccessful sensor configuration routines e.g. if the sensor is disconnected"/>
+  <arg name="lidar_port" default="0" doc="The port to which the sensor should send lidar data"/>
+  <arg name="imu_port" default="0" doc="The port to which the sensor should send imu data"/>
+  <arg name="udp_profile_lidar" default=" " doc="The lidar packet profile; possible values: {
     LEGACY,
     RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
@@ -70,6 +71,7 @@
       args="load ouster_ros/OusterSensor os_nodelet_mgr">
       <param name="~/sensor_hostname" type="str" value="$(arg sensor_hostname)"/>
       <param name="~/udp_dest" type="str" value="$(arg udp_dest)"/>
+      <param name="~/retry_configuration" type="bool" value="$(arg retry_configuration)"/>
       <param name="~/lidar_port" type="int" value="$(arg lidar_port)"/>
       <param name="~/imu_port" type="int" value="$(arg imu_port)"/>
       <param name="~/udp_profile_lidar" type="str" value="$(arg udp_profile_lidar)"/>

launch/sensor.launch

@@ -1,25 +1,26 @@
 <launch>
 
   <arg name="ouster_ns" default="ouster" doc="Override the default namespace of all ouster nodes"/>
-  <arg name="sensor_hostname" doc="hostname or IP in dotted decimal form of the sensor"/>
-  <arg name="udp_dest" default=" " doc="hostname or IP where the sensor will send data packets"/>
-  <arg name="lidar_port" default="0" doc="port to which the sensor should send lidar data"/>
-  <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
-  <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
+  <arg name="sensor_hostname" doc="The hostname or IP in dotted decimal form of the sensor"/>
+  <arg name="udp_dest" default=" " doc="The hostname or IP where the sensor will send data packets"/>
+  <arg name="retry_configuration" default="false" doc="True to re-attempt unsuccessful sensor configuration routines e.g. if the sensor is disconnected"/>
+  <arg name="lidar_port" default="0" doc="The port to which the sensor should send lidar data"/>
+  <arg name="imu_port" default="0" doc="The port to which the sensor should send imu data"/>
+  <arg name="udp_profile_lidar" default=" " doc="The lidar packet profile; possible values: {
     LEGACY,
     RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
     }"/>
-  <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
+  <arg name="lidar_mode" default=" " doc="The resolution and rate; possible values: {
     512x10,
     512x20,
     1024x10,
     1024x20,
     2048x10,
     4096x5
     }"/>
-  <arg name="timestamp_mode" default=" " doc="method used to timestamp measurements; possible values: {
+  <arg name="timestamp_mode" default=" " doc="The method used to timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
     TIME_FROM_PTP_1588,
@@ -45,7 +46,7 @@
     would set lidar_frame to be the frame used when publishing these messages."/>
 
   <arg name="no_bond" default="false"
-    doc="request no bond setup when nodelets are created"/>
+    doc="The request no bond setup when nodelets are created"/>
   <arg if="$(arg no_bond)" name="_no_bond" value="--no-bond"/>
   <arg unless="$(arg no_bond)" name="_no_bond" value=" "/>
 
@@ -74,6 +75,7 @@
       args="load ouster_ros/OusterSensor os_nodelet_mgr $(arg _no_bond)">
       <param name="~/sensor_hostname" type="str" value="$(arg sensor_hostname)"/>
       <param name="~/udp_dest" type="str" value="$(arg udp_dest)"/>
+      <param name="~/retry_configuration" type="bool" value="$(arg retry_configuration)"/>
       <param name="~/lidar_port" type="int" value="$(arg lidar_port)"/>
       <param name="~/imu_port" type="int" value="$(arg imu_port)"/>
       <param name="~/udp_profile_lidar" type="str" value="$(arg udp_profile_lidar)"/>

launch/sensor_mtp.launch

@@ -1,9 +1,10 @@
 <launch>
 
   <arg name="ouster_ns" default="ouster" doc="Override the default namespace of all ouster nodes"/>
-  <arg name="sensor_hostname" doc="hostname or IP in dotted decimal form of the sensor"/>
-  <arg name="udp_dest" doc="hostname or multicast group IP where the sensor will send UDP data packets"/>
-  <arg name="mtp_dest" default=" " doc="hostname IP address for receiving data packets via multicast,
+  <arg name="sensor_hostname" doc="The hostname or IP in dotted decimal form of the sensor"/>
+  <arg name="udp_dest" doc="The hostname or multicast group IP where the sensor will send UDP data packets"/>
+  <arg name="retry_configuration" default="false" doc="True to re-attempt unsuccessful sensor configuration routines e.g. if the sensor is disconnected"/>
+  <arg name="mtp_dest" default=" " doc="The hostname IP address for receiving data packets via multicast,
     by default it is INADDR_ANY, so packets will be received on first available interface"/>
   <arg name="mtp_main" default="false" doc="if true, then configure and reinit the sensor, otherwise
     start client with active configuration of sensor"/>
@@ -15,25 +16,25 @@
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
     }"/>
-  <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
+  <arg name="lidar_mode" default=" " doc="The resolution and rate; possible values: {
     512x10,
     512x20,
     1024x10,
     1024x20,
     2048x10,
     4096x5
     }"/>
-  <arg name="timestamp_mode" default=" " doc="method used to timestamp measurements; possible values: {
+  <arg name="timestamp_mode" default=" " doc="The method used to timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
   <arg name="ptp_utc_tai_offset" default="-37.0"
-    doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
+    doc="The UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
 
-  <arg name="metadata" default=" " doc="path to write metadata file when receiving sensor data"/>
-  <arg name="viz" default="true" doc="whether to run a rviz"/>
+  <arg name="metadata" default=" " doc="The path to write metadata file when receiving sensor data"/>
+  <arg name="viz" default="true" doc="Whether to run an RViz instance"/>
   <arg name="rviz_config" default="$(find ouster_ros)/config/viz.rviz" doc="optional rviz config file"/>
 
   <arg name="tf_prefix" default=" " doc="namespace for tf transforms"/>
@@ -79,6 +80,7 @@
       args="load ouster_ros/OusterSensor os_nodelet_mgr $(arg _no_bond)">
       <param name="~/sensor_hostname" type="str" value="$(arg sensor_hostname)"/>
       <param name="~/udp_dest" type="str" value="$(arg udp_dest)"/>
+      <param name="~/retry_configuration" type="bool" value="$(arg retry_configuration)"/>
       <param name="~/mtp_dest" type="str" value="$(arg mtp_dest)"/>
       <param name="~/mtp_main" type="bool" value="$(arg mtp_main)"/>
       <param name="~/lidar_port" type="int" value="$(arg lidar_port)"/>

src/os_sensor_nodelet.cpp

@@ -40,9 +40,10 @@ void OusterSensor::halt() {
 }
 
 void OusterSensor::onInit() {
+    bool retry_configuration = false;
     sensor_hostname = get_sensor_hostname();
-    sensor::sensor_config config = parse_config_from_ros_parameters();
-    configure_sensor(sensor_hostname, config);
+    config = parse_config_from_ros_parameters(retry_configuration);
+    configure_sensor(sensor_hostname, config, retry_configuration);
     sensor_client = create_sensor_client(sensor_hostname, config);
     if (!sensor_client) {
         auto error_msg = "Failed to initialize client";
@@ -74,7 +75,6 @@ std::string OusterSensor::get_sensor_hostname() {
 }
 
 void OusterSensor::update_config_and_metadata(sensor::client& cli) {
-    sensor::sensor_config config;
     auto success = get_config(sensor_hostname, config);
     if (!success) {
         active_config.clear();
@@ -223,7 +223,7 @@ std::shared_ptr<sensor::client> OusterSensor::create_sensor_client(
     return cli;
 }
 
-sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
+sensor::sensor_config OusterSensor::parse_config_from_ros_parameters(bool& retry_configuration) {
     auto& nh = getPrivateNodeHandle();
     auto udp_dest = nh.param("udp_dest", std::string{});
     auto mtp_dest_arg = nh.param("mtp_dest", std::string{});
@@ -233,6 +233,8 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
     auto lidar_mode_arg = nh.param("lidar_mode", std::string{});
     auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{});
     auto udp_profile_lidar_arg = nh.param("udp_profile_lidar", std::string{});
+
+    retry_configuration = nh.param("retry_configuration", false);
 
     if (lidar_port < 0 || lidar_port > 65535) {
         auto error_msg =
@@ -295,7 +297,6 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
         }
     }
 
-    sensor::sensor_config config;
     if (lidar_port == 0) {
         NODELET_WARN_COND(
             !is_arg_set(mtp_dest_arg),
@@ -330,7 +331,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
 }
 
 sensor::sensor_config OusterSensor::parse_config_from_staged_config_string() {
-    sensor::sensor_config config = sensor::parse_config(staged_config);
+    config = sensor::parse_config(staged_config);
     staged_config.clear();
     return config;
 }
@@ -366,27 +367,41 @@ uint8_t OusterSensor::compose_config_flags(
     return config_flags;
 }
 
-void OusterSensor::configure_sensor(const std::string& hostname,
-                                    sensor::sensor_config& config) {
-    if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) &&
-        !mtp_main) {
-        if (!get_config(hostname, config, true)) {
-            NODELET_ERROR("Error getting active config");
-        } else {
-            NODELET_INFO("Retrived active config of sensor");
+bool OusterSensor::configure_sensor(const std::string& hostname,
+                                    sensor::sensor_config& config,
+                                    const bool retry_configuration) {
+    bool is_configured = false;
+    bool is_first_attempt = true;
+    do {
+        // Throttling
+        if (!is_first_attempt) {
+            ros::Duration(0.01).sleep();
+            if (config.udp_dest &&
+                sensor::in_multicast(config.udp_dest.value()) &&
+                !mtp_main) {
+                if (!get_config(hostname, config, true)) {
+                    NODELET_ERROR("Error getting active config");
+                } else {
+                    NODELET_INFO("Retrieved active config of sensor");
+                }
+            } else {
+                try {
+                    if (!set_config(hostname, config, config_flags)) {
+                        NODELET_ERROR("Error connecting to sensor: '%s'", hostname.c_str());
+                    } else {
+                        is_configured = true;
+                    }
+                } catch (const std::exception& e) {
+                    NODELET_ERROR("Error setting config:  %s", e.what());
+                }
+            }
         }
-        return;
-    }
-
-    uint8_t config_flags = compose_config_flags(config);
-    if (!set_config(hostname, config, config_flags)) {
-        auto error_msg = "Error connecting to sensor " + hostname;
-        NODELET_ERROR_STREAM(error_msg);
-        throw std::runtime_error(error_msg);
-    }
+        is_first_attempt = false;
+    } while (!is_configured && retry_configuration);
 
     NODELET_INFO_STREAM("Sensor " << hostname
-                                  << " was configured successfully");
+                                  << " was "
+                                  << (is_configured ? "" : "not ") << "configured successfully");
 }
 
 void OusterSensor::populate_metadata_defaults(
@@ -483,6 +498,8 @@ void OusterSensor::handle_lidar_packet(sensor::client& cli,
     lidar_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) {
         bool success = sensor::read_lidar_packet(cli, buffer, pf);
         if (success) {
+            had_reconnection_success = false;
+            first_lidar_data_rx = ros::Time::now();
             read_lidar_packet_errors = 0;
             if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) {
                 // TODO: short circut reset if no breaking changes occured?
@@ -519,6 +536,8 @@ void OusterSensor::handle_imu_packet(sensor::client& cli,
 
 void OusterSensor::connection_loop(sensor::client& cli,
                                    const sensor::packet_format& pf) {
+    static constexpr double TIMEOUT = 3.0;
+
     auto state = sensor::poll_client(cli);
     if (state == sensor::EXIT) {
         NODELET_INFO("poll_client: caught signal, exiting!");
@@ -532,6 +551,16 @@ void OusterSensor::connection_loop(sensor::client& cli,
     if (state & sensor::LIDAR_DATA) {
         handle_lidar_packet(cli, pf);
     }
+    else if (!had_reconnection_success &&
+             (first_lidar_data_rx != ros::Time(0.0)) &&
+             (ros::Time::now().toSec() - first_lidar_data_rx.toSec()) > TIMEOUT) {
+        NODELET_ERROR("poll_client: attempting reconnection");
+        had_reconnection_success = configure_sensor(sensor_hostname, config, false);
+
+        if (had_reconnection_success) {
+            sensor_client = create_sensor_client(sensor_hostname, config);
+        }
+    }
     if (state & sensor::IMU_DATA) {
         handle_imu_packet(cli, pf);
     }
@@ -624,4 +653,4 @@ void OusterSensor::reactivate_sensor(bool /*init_id_reset*/) {
 
 }  // namespace ouster_ros
 
-PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterSensor, nodelet::Nodelet)
+PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterSensor, nodelet::Nodelet)

src/os_sensor_nodelet.h

@@ -116,6 +116,8 @@ class OusterSensor : public OusterSensorNodeletBase {
     std::string active_config;
     std::string mtp_dest;
     bool mtp_main;
+    bool had_reconnection_success = false;
+    sensor::sensor_config config;
     std::shared_ptr<sensor::client> sensor_client;
     PacketMsg lidar_packet;
     PacketMsg imu_packet;
@@ -124,6 +126,7 @@ class OusterSensor : public OusterSensorNodeletBase {
     ros::ServiceServer reset_srv;
     ros::ServiceServer get_config_srv;
     ros::ServiceServer set_config_srv;
+    ros::Time first_lidar_data_rx = ros::Time(0.0);
 
     // TODO: implement & utilize a lock-free ring buffer in future
     std::unique_ptr<ThreadSafeRingBuffer> lidar_packets;