From 87d6d49d5108d5b19c0e33d8984984d06403a297 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Tue, 12 Sep 2023 13:59:11 -0700 Subject: [PATCH] Shutdown when can not connect to sensor on startup in ROS2 (Humble/Iron) (#211) * Shutdown when can not connect to sensor on startup * Apply same behavior to independent launch file * Update changelog and package version --- CHANGELOG.rst | 1 + ouster-ros/launch/driver.launch.py | 32 +++++++++---------- ouster-ros/launch/sensor.composite.launch.py | 2 +- .../launch/sensor.independent.launch.py | 29 ++++++++--------- ouster-ros/package.xml | 2 +- 5 files changed, 32 insertions(+), 34 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 9757c10a..dcec3f6e 100644 --- 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 index 17b894a7..9da3b1b4 100644 --- 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 index 0326e4a3..5932403f 100644 --- 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 index 6a75656f..9618ee14 100644 --- 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 index 5abf165c..2163039d 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.10.2 + 0.10.3 Ouster ROS2 driver ouster developers BSD