ROS-119: ouster-ros driver automatic reconnection [HUMBLE/IRON/JAZZY] (…

…#362)

* Port sensor reconnection logic to ROS 2
* Add Jazzy to the build!
* Update README and checkout
* Add a note about not being able to properly handle invalid configuration
* Implement automatic start for sensor/record modes

.github/workflows/docker-image.yml

@@ -18,8 +18,9 @@ jobs:
           - rolling
           - humble
           - iron
+          - jazzy
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           submodules: true
       - name: Build the Docker image

CHANGELOG.rst

@@ -15,9 +15,15 @@ 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.xml`` file.
+* Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg
+  documentation and the associated params to enable. Known Issues:
+  - Doesn't handle detect and handle invalid configurations
+* Added an automatic start mode to make it easier to start the node without using time actions.
+  - To disable set ``auto_start`` to ``false`` during launch
+
 
 ouster_ros v0.12.0
 ==================

README.md

@@ -1,15 +1,15 @@
 # Official ROS driver for Ouster sensors
 
 [ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) |
-[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
+[ROS2 (rolling/humble/iron/jazzy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
 [ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
 
 <p style="float: right;"><img width="20%" src="docs/images/logo.png" /></p>
 
 | ROS Version | Build Status (Linux) |
 |:-----------:|:------:|
 | ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
-| ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
+| ROS2 (rolling/humble/iron/jazzy) | [![rolling/humble/iron/jazzy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
 | ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
 
 - [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors)
@@ -57,9 +57,9 @@ You can obtain detailed specs sheet about the sensors and obtain updated FW thro
 [downloads](https://ouster.com/downloads) section.
 
 ## Requirements
-This branch is only intended for use with **Rolling**, **Humble** and **Iron** ROS 2 distros. Please
-refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with the
-remainder of this guide.
+This branch is only intended for use with **Rolling**, **Humble**, **Iron** and **Jazzy** ROS 2 distros.
+Please refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with
+the remainder of this guide.
 
 > **Note**  
 > If you have _rosdep_ tool installed on your system you can then use the following command to get all
@@ -77,7 +77,7 @@ sudo apt install -y             \
     ros-$ROS_DISTRO-tf2-eigen   \
     ros-$ROS_DISTRO-rviz2
 ```
-where `$ROS_DISTRO` can be either ``rolling``, ``humble`` or ``iron``.
+where `$ROS_DISTRO` can be either ``rolling``, ``humble``, ``iron`` or ``jazzy``.
 
 > **Note**  
 > Installing `ros-$ROS_DISTRO-rviz` package is optional in case you didn't need to visualize the
@@ -120,7 +120,7 @@ git clone -b ros2 --recurse-submodules https://github.com/ouster-lidar/ouster-ro
 
 Next to compile the driver you need to source the ROS environemt into the active termainl:
 ```bash
-source /opt/ros/<ros-distro>/setup.bash # replace ros-distro with 'rolling', 'humble', or 'iron'
+source /opt/ros/<ros-distro>/setup.bash # replace ros-distro with 'rolling', 'humble', 'iron' or 'jazzy'
 ```
 
 Finally, invoke `colcon build` command from within the catkin workspace as shown below:

ouster-ros/config/driver_params.yaml

@@ -93,3 +93,12 @@ ouster/os_driver:
     azimuth_window_end: 360000
     # persist_config[optional]: request the sensor to persist settings
     persist_config: false
+    # attempt_config[optional]: attempting to reconnect to the sensor after
+    # connection loss or sensor powered down
+    attempt_reconnect: false
+    # dormant_period_between_reconnects[optional]: wait time in seconds between
+    # reconnection attempts
+    dormant_period_between_reconnects: 1.0
+    # max_failed_reconnect_attempts[optional]: maximum number of attempts trying
+    # to communicate with the sensor. Counter resets upon successful connection.
+    max_failed_reconnect_attempts: 2147483647

ouster-ros/config/os_sensor_cloud_image_params.yaml

@@ -14,10 +14,13 @@ ouster/os_sensor:
     metadata: ''
     lidar_port: 0
     imu_port: 0
-    use_system_default_qos: False
+    use_system_default_qos: false
     azimuth_window_start: 0
     azimuth_window_end: 360000
     persist_config: false
+    attempt_reconnect: false
+    dormant_period_between_reconnects: 1.0
+    max_failed_reconnect_attempts: 2147483647
 ouster/os_cloud:
   ros__parameters:
     sensor_frame: os_sensor
@@ -27,9 +30,9 @@ ouster/os_cloud:
     timestamp_mode: ''  # this value needs to match os_sensor/timestamp_mode
     ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588
     proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options
-    use_system_default_qos: False # needs to match the value defined for os_sensor node
+    use_system_default_qos: false # needs to match the value defined for os_sensor node
     scan_ring: 0  # Use this parameter in conjunction with the SCAN flag and choose a
                   # value the range [0, sensor_beams_count)
     point_type: original # choose from: {original, native, xyz, xyzi, xyzir}
 ouster/os_image:
-    use_system_default_qos: False # needs to match the value defined for os_sensor node
+    use_system_default_qos: false # needs to match the value defined for os_sensor node

ouster-ros/include/ouster_ros/os_sensor_node_base.h

@@ -30,7 +30,7 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
 
     void create_get_metadata_service();
 
-    void create_metadata_publisher();
+    void create_metadata_pub();
 
     void publish_metadata();
 

ouster-ros/launch/record.composite.launch.xml

@@ -60,9 +60,7 @@
     description="Use the default system QoS settings"/>
 
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
-    The IMG flag here is not supported and does not affect anything,
-    to disable image topics you would need to omit the os_image node
-    from the launch file"/>
+    use any combination of the 4 flags to enable or disable specific processors"/>
 
   <arg name="scan_ring" default="0" description="
     use this parameter in conjunction with the SCAN flag
@@ -85,6 +83,18 @@
   <arg name="persist_config" default="false"
     description="request the sensor to persist settings"/>
 
+  <arg name="attempt_reconnect" default="false"
+    description="attempting to reconnect to the sensor after connection loss or
+    sensor powered down"/>
+  <arg name="dormant_period_between_reconnects" default="1.0"
+    description="wait time in seconds between reconnection attempts"/>
+  <arg name="max_failed_reconnect_attempts" default="2147483647"
+    description="maximum number of attempts trying to communicate with the sensor.
+         Counter resets upon successful connection"/>
+
+  <arg name="auto_start" default="true"
+    description="automatically configure and activate the node"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
@@ -103,6 +113,12 @@
         <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
         <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
         <param name="persist_config" value="$(var persist_config)"/>
+        <param name="attempt_reconnect" value="$(var attempt_reconnect)"/>
+        <param name="dormant_period_between_reconnects"
+              value="$(var dormant_period_between_reconnects)"/>
+        <param name="max_failed_reconnect_attempts"
+              value="$(var max_failed_reconnect_attempts)"/>
+        <param name="auto_start" value="$(var auto_start)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterCloud" name="os_cloud">
         <param name="sensor_frame" value="$(var sensor_frame)"/>
@@ -118,17 +134,11 @@
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+        <param name="proc_mask" value="$(var proc_mask)"/>
       </composable_node>
     </node_container>
   </group>
 
-  <!-- HACK: configure and activate the sensor node via a process execute since state
-    transition is currently not availabe through launch.xml format -->
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_sensor configure"
-    launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_sensor activate"
-    launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
-
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>

ouster-ros/launch/replay.composite.launch.xml

@@ -47,9 +47,7 @@
     description="Use the default system QoS settings"/>
 
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
-    The IMG flag here is not supported and does not affect anything,
-    to disable image topics you would need to omit the os_image node
-    from the launch file"/>
+    use any combination of the 4 flags to enable or disable specific processors"/>
 
   <arg name="scan_ring" default="0" description="
     use this parameter in conjunction with the SCAN flag
@@ -85,6 +83,7 @@
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+        <param name="proc_mask" value="$(var proc_mask)"/>
       </composable_node>
     </node_container>
   </group>

ouster-ros/launch/replay_pcap.launch.xml

@@ -38,9 +38,7 @@
     description="Use the default system QoS settings"/>
 
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
-    The IMG flag here is not supported and does not affect anything,
-    to disable image topics you would need to omit the os_image node
-    from the launch file"/>
+    use any combination of the 4 flags to enable or disable specific processors"/>
 
   <arg name="scan_ring" default="0" description="
     use this parameter in conjunction with the SCAN flag
@@ -77,6 +75,7 @@
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+      <param name="proc_mask" value="$(var proc_mask)"/>
       </composable_node>
     </node_container>
   </group>

ouster-ros/launch/sensor.composite.launch.py

@@ -36,12 +36,16 @@ def generate_launch_description():
     rviz_enable = LaunchConfiguration('viz')
     rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True')
 
+    auto_start = LaunchConfiguration('auto_start')
+    auto_start_arg = DeclareLaunchArgument('auto_start', default_value='True')
+
     os_sensor = ComposableNode(
         package='ouster_ros',
         plugin='ouster_ros::OusterSensor',
         name='os_sensor',
         namespace=ouster_ns,
-        parameters=[params_file]
+        parameters=[params_file,
+        {'auto_start': auto_start}]
     )
 
     os_cloud = ComposableNode(
@@ -80,25 +84,11 @@ def generate_launch_description():
         condition=IfCondition(rviz_enable)
     )
 
-    # HACK: to configure and activate the the sensor since state transition
-    # API doesn't seem to support composable nodes yet.
-
-    def invoke_lifecycle_cmd(node_name, verb):
-        ros2_exec = FindExecutable(name='ros2')
-        return ExecuteProcess(
-            cmd=[[ros2_exec, ' lifecycle set ',
-                  ouster_ns, '/', node_name, ' ', verb]],
-            shell=True)
-
-    sensor_configure_cmd = invoke_lifecycle_cmd('os_sensor', 'configure')
-    sensor_activate_cmd = invoke_lifecycle_cmd('os_sensor', 'activate')
-
     return launch.LaunchDescription([
         params_file_arg,
         ouster_ns_arg,
         rviz_enable_arg,
+        auto_start_arg,
         rviz_launch,
-        os_container,
-        sensor_configure_cmd,
-        TimerAction(period=1.0, actions=[sensor_activate_cmd])
+        os_container
     ])

ouster-ros/launch/sensor.composite.launch.xml

@@ -81,6 +81,18 @@
   <arg name="persist_config" default="false"
     description="request the sensor to persist settings"/>
 
+  <arg name="attempt_reconnect" default="false"
+    description="attempting to reconnect to the sensor after connection loss or
+    sensor powered down"/>
+  <arg name="dormant_period_between_reconnects" default="1.0"
+    description="wait time in seconds between reconnection attempts"/>
+  <arg name="max_failed_reconnect_attempts" default="2147483647"
+    description="maximum number of attempts trying to communicate with the sensor.
+         Counter resets upon successful connection"/>
+
+  <arg name="auto_start" default="true"
+    description="automatically configure and activate the node"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_driver" name="os_driver" output="screen">
@@ -106,16 +118,15 @@
       <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
       <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
       <param name="persist_config" value="$(var persist_config)"/>
+      <param name="attempt_reconnect" value="$(var attempt_reconnect)"/>
+      <param name="dormant_period_between_reconnects"
+             value="$(var dormant_period_between_reconnects)"/>
+      <param name="max_failed_reconnect_attempts"
+             value="$(var max_failed_reconnect_attempts)"/>
+      <param name="auto_start" value="$(var auto_start)"/>
     </node>
   </group>
 
-  <!-- HACK: configure and activate the sensor node via a process execute since state
-    transition is currently not availabe through launch.xml format -->
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_driver configure"
-    launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_driver activate"
-    launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
-
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>

ouster-ros/launch/sensor.independent.launch.xml

@@ -58,9 +58,7 @@
     description="Use the default system QoS settings"/>
 
   <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
-    The IMG flag here is not supported and does not affect anything,
-    to disable image topics you would need to omit the os_image node
-    from the launch file"/>
+    use any combination of the 4 flags to enable or disable specific processors"/>
 
   <arg name="scan_ring" default="0" description="
     use this parameter in conjunction with the SCAN flag
@@ -83,6 +81,18 @@
   <arg name="persist_config" default="false"
     description="request the sensor to persist settings"/>
 
+  <arg name="attempt_reconnect" default="false"
+    description="attempting to reconnect to the sensor after connection loss or
+    sensor powered down"/>
+  <arg name="dormant_period_between_reconnects" default="1.0"
+    description="wait time in seconds between reconnection attempts"/>
+  <arg name="max_failed_reconnect_attempts" default="2147483647"
+    description="maximum number of attempts trying to communicate with the sensor.
+         Counter resets upon successful connection"/>
+
+  <arg name="auto_start" default="true"
+    description="automatically configure and activate the node"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_sensor" name="os_sensor" output="screen">
@@ -100,6 +110,12 @@
       <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
       <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
       <param name="persist_config" value="$(var persist_config)"/>
+      <param name="attempt_reconnect" value="$(var attempt_reconnect)"/>
+      <param name="dormant_period_between_reconnects"
+             value="$(var dormant_period_between_reconnects)"/>
+      <param name="max_failed_reconnect_attempts"
+             value="$(var max_failed_reconnect_attempts)"/>
+      <param name="auto_start" value="$(var auto_start)"/>
     </node>
     <node pkg="ouster_ros" exec="os_cloud" name="os_cloud" output="screen">
       <param name="sensor_frame" value="$(var sensor_frame)"/>
@@ -115,16 +131,10 @@
     </node>
     <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+      <param name="proc_mask" value="$(var proc_mask)"/>
     </node>
   </group>
 
-  <!-- HACK: configure and activate the sensor node via a process execute since state
-    transition is currently not availabe through launch.xml format -->
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_sensor configure"
-    launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_sensor activate"
-    launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
-
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>