From d0370d7b524bb727f1310b16a8638bec441d1b2f Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 May 2025 20:35:33 -0400 Subject: [PATCH] rplidar --- src/modelec_core/launch/modelec.launch.py | 33 ++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 36708e3..5abe368 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,5 +1,5 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown, RegisterEventHandler +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -8,7 +8,6 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory import os - def generate_launch_description(): # Declare launch arguments with_gui_arg = DeclareLaunchArgument('with_gui', default_value='true', description='Launch GUI?') @@ -34,6 +33,32 @@ def generate_launch_description(): condition=IfCondition(with_rplidar) ) + # Function to launch RPLIDAR node with crash detection and restart + def launch_rplidar_with_restart(context, *args, **kwargs): + if context.launch_configurations.get('with_rplidar') == 'true': + rplidar_node = Node( + package='rplidar_ros', + executable='rplidar_node', + name='rplidar_node', + output='screen', + ) + + # Register event handler to restart rplidar node only if it crashes (exit code non-nul) + restart_handler = RegisterEventHandler( + OnProcessExit( + target_action=rplidar_node, + on_exit=[ + TimerAction( + period=5.0, # Delay before restarting (in seconds) + actions=[rplidar_node] # Restart the rplidar_node + ) + ] + ) + ) + + return [rplidar_node, restart_handler] + return [] + # Function to launch GUI and shutdown handler def launch_gui(context, *args, **kwargs): if context.launch_configurations.get('with_gui') == 'true': @@ -77,7 +102,9 @@ def generate_launch_description(): with_com_arg, with_strat_arg, - rplidar_launch, + # Conditionally launch the RPLIDAR node with the restart handler + OpaqueFunction(function=launch_rplidar_with_restart), + OpaqueFunction(function=launch_gui), OpaqueFunction(function=launch_com), OpaqueFunction(function=launch_strat),