From 9d9bd332eedaabbd869d6f031fb7d3d90f6db309 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 19 Mar 2026 20:31:22 +0100 Subject: [PATCH] rewrite launch file --- src/modelec_core/launch/modelec.launch.py | 203 ++++++++++------------ 1 file changed, 88 insertions(+), 115 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index b08456b..2ed6f71 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,141 +1,114 @@ import os from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - OpaqueFunction, - Shutdown, - RegisterEventHandler, - TimerAction, - LogInfo, -) +from launch.actions import DeclareLaunchArgument, Shutdown, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # ------------------------------------------------- # Launch arguments # ------------------------------------------------- - with_gui_arg = DeclareLaunchArgument('with_gui', default_value='true') - with_rplidar_arg = DeclareLaunchArgument('with_rplidar', default_value='true') - with_com_arg = DeclareLaunchArgument('with_com', default_value='true') - with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true') - with_enemy_manager_arg = DeclareLaunchArgument('with_enemy_manager', default_value='true') - with_joy_arg = DeclareLaunchArgument('with_joy', default_value='true') - with_color_detector_arg = DeclareLaunchArgument('with_color_detector', default_value='true') + args = [ + DeclareLaunchArgument('with_gui', default_value='true'), + DeclareLaunchArgument('with_rplidar', default_value='true'), + DeclareLaunchArgument('with_com', default_value='true'), + DeclareLaunchArgument('with_strat', default_value='true'), + DeclareLaunchArgument('with_enemy_manager', default_value='true'), + DeclareLaunchArgument('with_joy', default_value='true'), + DeclareLaunchArgument('with_color_detector', default_value='true'), - channel_type = LaunchConfiguration('channel_type', default='serial') - serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') - serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') - frame_id = LaunchConfiguration('frame_id', default='laser') - inverted = LaunchConfiguration('inverted', default='false') - angle_compensate = LaunchConfiguration('angle_compensate', default='true') - scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + # RPLidar specific arguments + DeclareLaunchArgument('channel_type', default_value='serial'), + DeclareLaunchArgument('serial_port', default_value='/dev/ttyUSB0'), + DeclareLaunchArgument('serial_baudrate', default_value='115200'), + DeclareLaunchArgument('frame_id', default_value='laser'), + DeclareLaunchArgument('inverted', default_value='false'), + DeclareLaunchArgument('angle_compensate', default_value='true'), + DeclareLaunchArgument('scan_mode', default_value='Sensitivity'), + ] # ------------------------------------------------- - # RPLIDAR logic (Starts Immediately) + # Nodes Configuration # ------------------------------------------------- - def create_lidar_with_restart(): - 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}] - ) - restart_handler = RegisterEventHandler( - OnProcessExit( - target_action=lidar_node, - on_exit=[ - LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 5s...'), - TimerAction( - period=2.0, - actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())], - ), - ], - ) - ) - return [lidar_node, restart_handler] + # RPLIDAR Node + rplidar_node = Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_rplidar')), + package='rplidar_ros', + executable='rplidar_node', + name='rplidar_node', + parameters=[{ + 'channel_type': LaunchConfiguration('channel_type'), + 'serial_port': LaunchConfiguration('serial_port'), + 'serial_baudrate': LaunchConfiguration('serial_baudrate'), + 'frame_id': LaunchConfiguration('frame_id'), + 'inverted': LaunchConfiguration('inverted'), + 'angle_compensate': LaunchConfiguration('angle_compensate'), + 'scan_mode': LaunchConfiguration('scan_mode') + }] + ) - def launch_rplidar_now(context, *args, **kwargs): - if context.launch_configurations.get('with_rplidar') == 'true': - return create_lidar_with_restart() - return [] + # GUI Node (with Shutdown on exit) + gui_node = Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_gui')), + package='modelec_gui', + executable='modelec_gui', + name='modelec_gui', + on_exit=Shutdown() + ) - # ------------------------------------------------- - # Helper to delay other nodes - # ------------------------------------------------- - def delay_launch(context, function_to_call): - return [ - TimerAction( - period=15.0, - actions=function_to_call(context) - ) - ] + # Communication Nodes + com_nodes = [ + Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_com')), + package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface' + ), + Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_com')), + package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface' + ), + ] - # ------------------------------------------------- - # Node Launch Functions - # ------------------------------------------------- - def launch_gui(context, *args, **kwargs): - if context.launch_configurations.get('with_gui') == 'true': - gui = Node(package='modelec_gui', executable='modelec_gui', name='modelec_gui') - return [gui, RegisterEventHandler(OnProcessExit(target_action=gui, on_exit=[Shutdown()]))] - return [] + # Strategy Nodes + strat_nodes = [ + Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_strat')), + package='modelec_strat', executable='strat_fsm', name='strat_fsm' + ), + Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_strat')), + package='modelec_strat', executable='pami_manager', name='pami_manager' + ), + ] - def launch_com(context, *args, **kwargs): - if context.launch_configurations.get('with_com') == 'true': - return [ - Node(package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface'), - Node(package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface'), - ] - return [] + # Enemy Manager + enemy_manager_node = Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_enemy_manager')), + package='modelec_strat', executable='enemy_manager', name='enemy_manager' + ) - def launch_strat(context, *args, **kwargs): - if context.launch_configurations.get('with_strat') == 'true': - return [ - Node(package='modelec_strat', executable='strat_fsm', name='strat_fsm'), - Node(package='modelec_strat', executable='pami_manager', name='pami_manager'), - ] - return [] + # Joystick + joy_node = Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_joy')), + package='joy', executable='joy_node', name='joy_node' + ) - def launch_enemy_manager(context, *args, **kwargs): - if context.launch_configurations.get('with_enemy_manager') == 'true': - return [Node(package='modelec_strat', executable='enemy_manager', name='enemy_manager')] - return [] - - def launch_joy(context, *args, **kwargs): - if context.launch_configurations.get('with_joy') == 'true': - return [Node(package='joy', executable='joy_node', name='joy_node')] - return [] - - def launch_color_detector(context, *args, **kwargs): - if context.launch_configurations.get('with_color_detector') == 'true': - return [Node(package='modelec_com', executable='color_detector', name='color_detector')] - return [] + # Color Detector + color_detector_node = Node( + condition=launch_ros.conditions.IfCondition(LaunchConfiguration('with_color_detector')), + package='modelec_com', executable='color_detector', name='color_detector' + ) # ------------------------------------------------- # Final LaunchDescription # ------------------------------------------------- - return LaunchDescription([ - # Arguments - with_gui_arg, with_rplidar_arg, with_com_arg, with_strat_arg, - with_enemy_manager_arg, with_joy_arg, with_color_detector_arg, - - # Start Lidar Immediately - OpaqueFunction(function=launch_rplidar_now), - - # Start everything else after 3 seconds - OpaqueFunction(function=lambda context: delay_launch(context, launch_gui)), - OpaqueFunction(function=lambda context: delay_launch(context, launch_com)), - OpaqueFunction(function=lambda context: delay_launch(context, launch_strat)), - OpaqueFunction(function=lambda context: delay_launch(context, launch_enemy_manager)), - OpaqueFunction(function=lambda context: delay_launch(context, launch_joy)), - OpaqueFunction(function=lambda context: delay_launch(context, launch_color_detector)), + return LaunchDescription(args + [ + rplidar_node, + gui_node, + *com_nodes, + *strat_nodes, + enemy_manager_node, + joy_node, + color_detector_node ]) \ No newline at end of file