Shutdown when can not connect to sensor on startup in ROS2 (Humble/Ir…

…on) (#211) * Shutdown when can not connect to sensor on startup * Apply same behavior to independent launch file * Update changelog and package version
ouster-lidar · Sep 12, 2023 · 87d6d49 · 87d6d49
1 parent 19071eb
commit 87d6d49
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 34 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -9,6 +9,7 @@ Changelog
   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.
+* shutdown the driver when unable to connect to the sensor on startup
 
 
 ouster_ros v0.10.0

diff --git a/ouster-ros/launch/driver.launch.py b/ouster-ros/launch/driver.launch.py
@@ -7,7 +7,7 @@
 import launch
 import lifecycle_msgs.msg
 from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node, LifecycleNode
+from launch_ros.actions import LifecycleNode
 from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
                             RegisterEventHandler, EmitEvent, LogInfo)
 from launch.conditions import IfCondition
@@ -40,7 +40,8 @@ def generate_launch_description():
     rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True')
 
     os_driver_name = LaunchConfiguration('os_driver_name')
-    os_driver_name_arg = DeclareLaunchArgument('os_driver_name', default_value='os_driver')
+    os_driver_name_arg = DeclareLaunchArgument(
+        'os_driver_name', default_value='os_driver')
 
     os_driver = LifecycleNode(
         package='ouster_ros',
@@ -72,20 +73,17 @@ def generate_launch_description():
         )
     )
 
-    # TODO: figure out why registering for on_shutdown event causes an exception
-    # and error handling
-    # shutdown_event = RegisterEventHandler(
-    #     OnShutdown(
-    #         on_shutdown=[
-    #             EmitEvent(event=ChangeState(
-    #               lifecycle_node_matcher=matches_node_name(node_name=F"/ouster/os_sensor"),
-    #               transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN,
-    #             )),
-    #             LogInfo(msg="os_sensor node exiting..."),
-    #         ],
-    #     )
-    # )
-
+    sensor_finalized_event = RegisterEventHandler(
+        OnStateTransition(
+            target_lifecycle_node=os_driver, goal_state='finalized',
+            entities=[
+                LogInfo(
+                    msg="Failed to communicate with the sensor in a timely manner."),
+                EmitEvent(event=launch.events.Shutdown(
+                    reason="Couldn't communicate with sensor"))
+            ],
+        )
+    )
 
     rviz_launch_file_path = \
         Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py'
@@ -103,5 +101,5 @@ def generate_launch_description():
         os_driver,
         sensor_configure_event,
         sensor_activate_event,
-        # shutdown_event
+        sensor_finalized_event
     ])
diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py
@@ -6,7 +6,7 @@
 from pathlib import Path
 import launch
 from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
                             ExecuteProcess, TimerAction)

diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py
@@ -25,7 +25,8 @@ def generate_launch_description():
     """
     ouster_ros_pkg_dir = get_package_share_directory('ouster_ros')
     default_params_file = \
-        Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml'
+        Path(ouster_ros_pkg_dir) / 'config' / \
+        'os_sensor_cloud_image_params.yaml'
     params_file = LaunchConfiguration('params_file')
     params_file_arg = DeclareLaunchArgument('params_file',
                                             default_value=str(
@@ -69,19 +70,17 @@ def generate_launch_description():
         )
     )
 
-    # TODO: figure out why registering for on_shutdown event causes an exception
-    # and error handling
-    # shutdown_event = RegisterEventHandler(
-    #     OnShutdown(
-    #         on_shutdown=[
-    #             EmitEvent(event=ChangeState(
-    #               lifecycle_node_matcher=matches_node_name(node_name=F"/ouster/os_sensor"),
-    #               transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN,
-    #             )),
-    #             LogInfo(msg="os_sensor node exiting..."),
-    #         ],
-    #     )
-    # )
+    sensor_finalized_event = RegisterEventHandler(
+        OnStateTransition(
+            target_lifecycle_node=os_sensor, goal_state='finalized',
+            entities=[
+                LogInfo(
+                    msg="Failed to communicate with the sensor in a timely manner."),
+                EmitEvent(event=launch.events.Shutdown(
+                    reason="Couldn't communicate with sensor"))
+            ],
+        )
+    )
 
     os_cloud = Node(
         package='ouster_ros',
@@ -118,5 +117,5 @@ def generate_launch_description():
         os_image,
         sensor_configure_event,
         sensor_activate_event,
-        # shutdown_event
+        sensor_finalized_event
     ])
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.10.2</version>
+  <version>0.10.3</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>