From 3d7bfca61e0173424dad8fb1cc437f2e4e002656 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 12 Nov 2025 18:31:38 +0100 Subject: [PATCH] launch file ROS2 --- src/modelec_core/launch/modelec.launch.py | 194 ++++++++++------------ 1 file changed, 84 insertions(+), 110 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index bd59deb..bbad77b 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,12 +1,8 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction -from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction, LogInfo from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import os def generate_launch_description(): @@ -16,7 +12,7 @@ def generate_launch_description(): with_com_arg = DeclareLaunchArgument('with_com', default_value='true', description='Launch COM nodes?') with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true', description='Launch strategy nodes?') - channel_type = LaunchConfiguration('channel_type', default='serial') + channel_type = LaunchConfiguration('channel_type', default='serial') serial_port = LaunchConfiguration('serial_port', default='/dev/rplidar') serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') frame_id = LaunchConfiguration('frame_id', default='laser') @@ -24,49 +20,57 @@ def generate_launch_description(): angle_compensate = LaunchConfiguration('angle_compensate', default='true') scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') - # Get launch configs - with_gui = LaunchConfiguration('with_gui') - with_rplidar = LaunchConfiguration('with_rplidar') - with_com = LaunchConfiguration('with_com') - with_strat = LaunchConfiguration('with_strat') + # ---------------------------- + # Infinite LIDAR restart logic + # ---------------------------- + def create_lidar_with_restart(): + """Create a lidar node and its auto-restart handler.""" + lidar_node = Node( + package='rplidar_ros', + executable='rplidar_node', + name='rplidar_node', + parameters=[{ + 'channel_type': channel_type, + 'serial_port': serial_port, + 'serial_baudrate': serial_baudrate, + 'frame_id': frame_id, + 'inverted': inverted, + 'angle_compensate': angle_compensate, + 'scan_mode': scan_mode, + }], + output='screen' + ) + + # Register a restart handler that re-creates both node + handler + restart_handler = RegisterEventHandler( + OnProcessExit( + target_action=lidar_node, + on_exit=[ + LogInfo(msg="[Launch] RPLIDAR crashed — restarting in 2s..."), + TimerAction( + period=2.0, + actions=create_lidar_with_restart() # recursive: spawns new node + handler + ) + ] + ) + ) + + return [lidar_node, restart_handler] def launch_rplidar_restart_if_needed(context, *args, **kwargs): if context.launch_configurations.get('with_rplidar') == 'true': - def create_rplidar_node(): - return Node( - package='rplidar_ros', - executable='rplidar_node', - name='rplidar_node', - parameters=[{ - 'channel_type': channel_type, - 'serial_port': serial_port, - 'serial_baudrate': serial_baudrate, - 'frame_id': frame_id, - 'inverted': inverted, - 'angle_compensate': angle_compensate - }], - output='screen' + # Start after 3 seconds + return [ + TimerAction( + period=3.0, + actions=create_lidar_with_restart() ) - - rplidar_node = create_rplidar_node() - - restart_handler = RegisterEventHandler( - OnProcessExit( - target_action=rplidar_node, - on_exit=[ - TimerAction( - period=2.0, - actions=[create_rplidar_node()] - ) - ] - ) - ) - - return [rplidar_node, restart_handler] + ] return [] - - # Function to launch GUI and shutdown handler + # ---------------------------- + # GUI node + # ---------------------------- def launch_gui(context, *args, **kwargs): if context.launch_configurations.get('with_gui') == 'true': gui = Node( @@ -83,45 +87,39 @@ def generate_launch_description(): return [gui, shutdown] return [] - # Function to launch COM nodes + # ---------------------------- + # COM nodes + # ---------------------------- def launch_com(context, *args, **kwargs): if context.launch_configurations.get('with_com') == 'true': - serial_listener = Node( - package='modelec_com', - executable='serial_listener', - name='serial_listener' - ) - - pcb_odo_interface = Node( - package='modelec_com', - executable='pcb_odo_interface', - name='pcb_odo_interface', - parameters=[{ - 'serial_port': "/dev/USB_ODO", - 'baudrate': 115200, - 'name': "pcb_odo", - }] - ) - - pcb_action_interface = Node( - package='modelec_com', - executable='pcb_action_interface', - name='pcb_action_interface', - parameters=[{ - 'serial_port': "/dev/USB_ACTION", - 'baudrate': 115200, - 'name': "pcb_action", - }] - ) - return [ - serial_listener, - pcb_odo_interface, - pcb_action_interface, + Node(package='modelec_com', executable='serial_listener', name='serial_listener'), + Node( + package='modelec_com', + executable='pcb_odo_interface', + name='pcb_odo_interface', + parameters=[{ + 'serial_port': "/dev/USB_ODO", + 'baudrate': 115200, + 'name': "pcb_odo", + }] + ), + Node( + package='modelec_com', + executable='pcb_action_interface', + name='pcb_action_interface', + parameters=[{ + 'serial_port': "/dev/USB_ACTION", + 'baudrate': 115200, + 'name': "pcb_action", + }] + ), ] return [] - # Function to launch strategy nodes + # ---------------------------- + # Strategy nodes + # ---------------------------- def launch_strat(context, *args, **kwargs): if context.launch_configurations.get('with_strat') == 'true': return [ @@ -131,41 +129,17 @@ def generate_launch_description(): ] return [] + # ---------------------------- + # Final launch description + # ---------------------------- return LaunchDescription([ - - DeclareLaunchArgument( - 'channel_type', - default_value=channel_type, - description='Specifying channel type of lidar'), - - DeclareLaunchArgument( - 'serial_port', - default_value=serial_port, - description='Specifying usb port to connected lidar'), - - DeclareLaunchArgument( - 'serial_baudrate', - default_value=serial_baudrate, - description='Specifying usb port baudrate to connected lidar'), - - DeclareLaunchArgument( - 'frame_id', - default_value=frame_id, - description='Specifying frame_id of lidar'), - - DeclareLaunchArgument( - 'inverted', - default_value=inverted, - description='Specifying whether or not to invert scan data'), - - DeclareLaunchArgument( - 'angle_compensate', - default_value=angle_compensate, - description='Specifying whether or not to enable angle_compensate of scan data'), - DeclareLaunchArgument( - 'scan_mode', - default_value=scan_mode, - description='Specifying scan mode of lidar'), + DeclareLaunchArgument('channel_type', default_value=channel_type, description='Specifying channel type of lidar'), + DeclareLaunchArgument('serial_port', default_value=serial_port, description='Specifying usb port to connected lidar'), + DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate, description='Specifying usb port baudrate to connected lidar'), + DeclareLaunchArgument('frame_id', default_value=frame_id, description='Specifying frame_id of lidar'), + DeclareLaunchArgument('inverted', default_value=inverted, description='Specifying whether or not to invert scan data'), + DeclareLaunchArgument('angle_compensate', default_value=angle_compensate, description='Specifying whether or not to enable angle_compensate of scan data'), + DeclareLaunchArgument('scan_mode', default_value=scan_mode, description='Specifying scan mode of lidar'), with_gui_arg, with_rplidar_arg,