#!/usr/bin/env python3 # -*- coding: utf-8 -*- # ============================================================================ # master_launch.py —— 机器人系统总启动文件 # 启动顺序: origincar_base → LiDAR → TTS服务 → USB相机+二维码 → 障碍物检测 → 路径规划 → 轨迹跟随 → VLM # 用法: # ros2 launch my_robot_bringup master_launch.py # ros2 launch my_robot_bringup master_launch.py use_qr:=false # ros2 launch my_robot_bringup master_launch.py use_vlm:=false # ros2 launch my_robot_bringup master_launch.py use_planner:=false # ============================================================================ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, LogInfo, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # ================================================================ # 路径 # ================================================================ origincar_dir = get_package_share_directory('origincar_base') lslidar_dir = get_package_share_directory('lslidar_driver') obstacle_dir = get_package_share_directory('obstacle_scanner') planner_dir = get_package_share_directory('planner') vlm_dir = get_package_share_directory('vlm_detect') # ================================================================ # Launch 参数 —— 可通过命令行灵活切换 # ================================================================ # 模块开关 use_base = LaunchConfiguration('use_base', default='true') use_lidar = LaunchConfiguration('use_lidar', default='true') use_qr = LaunchConfiguration('use_qr', default='true') use_tts = LaunchConfiguration('use_tts', default='true') use_obstacle = LaunchConfiguration('use_obstacle', default='true') use_planner = LaunchConfiguration('use_planner', default='true') use_vlm = LaunchConfiguration('use_vlm', default='true') # 车辆模式 akmcar = LaunchConfiguration('akmcar', default='true') carto_slam = LaunchConfiguration('carto_slam', default='false') # 摄像头设备 camera_device = LaunchConfiguration('camera_device', default='/dev/video0') # TTS 参数 audio_sink = LaunchConfiguration('audio_sink', default='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo') tts_speed = LaunchConfiguration('tts_speed', default='1.5') # 地图文件 (AMCL 定位用) map_file = LaunchConfiguration('map_file', default=os.path.join( planner_dir, 'maps', 'real_5x5.yaml')) # Planner / Tracker 参数文件 planner_params_file = LaunchConfiguration('planner_params_file', default=os.path.join( planner_dir, 'config', 'real_hybrid_astar.yaml')) tracker_params_file = LaunchConfiguration('tracker_params_file', default=os.path.join( planner_dir, 'config', 'real_pure_pursuit.yaml')) amcl_params_file = LaunchConfiguration('amcl_params_file', default=os.path.join( planner_dir, 'config', 'real_amcl.yaml')) # VLM 服务器地址 vlm_host = LaunchConfiguration('vlm_host', default='http://192.168.10.189:8000') # 通用 use_sim_time = LaunchConfiguration('use_sim_time', default='false') # ================================================================ # 阶段 1: 底盘驱动 + EKF + TF + IMU (t=0s) # ================================================================ origincar_bringup = IncludeLaunchDescription( PythonLaunchDescriptionSource([ origincar_dir, '/launch', '/origincar_bringup.launch.py' ]), condition=IfCondition(use_base), launch_arguments={ 'akmcar': akmcar, 'carto_slam': carto_slam, }.items(), ) # ================================================================ # 阶段 2: 激光雷达 (t=2s) # ================================================================ lidar_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ lslidar_dir, '/launch', '/lsn10_launch.py' ]), condition=IfCondition(use_lidar), ) # ================================================================ # 阶段 3: TTS 语音播报服务 + 二维码播报桥接 (t=3s) # tts_server: Piper 离线 TTS + espeak 降级，提供 /tts/speak 服务 # qr_tts_bridge: 订阅 qr_results，调用 /tts/speak # ================================================================ tts_server = Node( package='vlm_detect', executable='tts_server', name='tts_server', output='screen', condition=IfCondition(use_tts), parameters=[{ 'audio_sink': audio_sink, 'tts_speed': tts_speed, }], ) qr_tts_bridge = Node( package='vlm_detect', executable='qr_tts_bridge', name='qr_tts_bridge', output='screen', condition=IfCondition(use_tts), ) # ================================================================ # 阶段 4: USB 摄像头 + 二维码识别 (t=4s) # usb_cam: 驱动 USB 摄像头，发布 /image_raw # qr_detect: 订阅 /image_raw，识别二维码，发布 qr_results # ================================================================ usb_camera = Node( package='usb_cam', executable='usb_cam_node_exe', name='usb_cam', output='screen', condition=IfCondition(use_qr), parameters=[{ 'video_device': camera_device, 'image_size': [640, 480], 'pixel_format': 'YUYV', 'framerate': 30.0, }], ) qr_detect = Node( package='qr_detection', executable='qr_detect', name='qr_detect', output='screen', condition=IfCondition(use_qr), parameters=[{'image_topic': '/image_raw'}], ) # ================================================================ # 阶段 5: 障碍物检测 (t=5s) # ================================================================ obstacle_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ obstacle_dir, '/launch', '/obstacle_scanner.launch.py' ]), condition=IfCondition(use_obstacle), ) # ================================================================ # 阶段 6: 地图服务器 + AMCL 定位 (t=7s) # ================================================================ map_server_node = Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', condition=IfCondition(use_planner), parameters=[{'yaml_filename': map_file, 'use_sim_time': use_sim_time}], ) amcl_node = Node( package='nav2_amcl', executable='amcl', name='amcl', output='screen', condition=IfCondition(use_planner), parameters=[amcl_params_file, {'use_sim_time': use_sim_time}], ) lifecycle_manager = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', condition=IfCondition(use_planner), parameters=[{'use_sim_time': use_sim_time, 'autostart': True, 'node_names': ['map_server', 'amcl']}], ) # ================================================================ # 阶段 7: Hybrid A* 路径规划 + Pure Pursuit 轨迹跟随 (t=9s) # ================================================================ planner_node = Node( package='planner', executable='grid_astar_theta_node', name='grid_astar_theta_planner', output='screen', condition=IfCondition(use_planner), parameters=[planner_params_file, {'use_sim_time': use_sim_time, 'obstacles_topic': '/obstacles'}], ) tracker_node = Node( package='planner', executable='topology_pure_pursuit_node.py', name='topology_pure_pursuit', output='screen', condition=IfCondition(use_planner), parameters=[tracker_params_file, {'use_sim_time': use_sim_time}], ) # ================================================================ # 阶段 8: VLM 图生文 (t=11s) # vlm_node 内部自带 TTS 服务客户端，识别完自动调用 /tts/speak # 注意: TTS 服务和桥接已在阶段 3 启动，这里只启动 vlm_node # ================================================================ vlm_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ vlm_dir, '/launch', '/vlm_detect.launch.py' ]), condition=IfCondition(use_vlm), launch_arguments={ 'vlm_host': vlm_host, 'use_tts': 'false', # tts_server 已在阶段 3 启动 'use_qr_tts': 'false', # qr_tts_bridge 已在阶段 3 启动 }.items(), ) # ================================================================ # 组装 —— 通过 TimerAction 保证先后顺序 # ================================================================ return LaunchDescription([ # 模块 开关 参数 DeclareLaunchArgument('use_base', default_value='true', description='底盘驱动 + EKF + TF'), DeclareLaunchArgument('use_lidar', default_value='true', description='激光雷达 (lsn10)'), DeclareLaunchArgument('use_qr', default_value='true', description='USB 摄像头 + 二维码识别'), DeclareLaunchArgument('use_tts', default_value='true', description='TTS 语音播报服务 + 二维码播报桥接'), DeclareLaunchArgument('use_obstacle', default_value='true', description='障碍物检测'), DeclareLaunchArgument('use_planner', default_value='true', description='路径规划 + 轨迹跟随'), DeclareLaunchArgument('use_vlm', default_value='true', description='VLM 图生文'), DeclareLaunchArgument('akmcar', default_value='true', description='车辆模型 (true=阿克曼, false=麦轮)'), DeclareLaunchArgument('carto_slam', default_value='false', description='使用 Cartographer 替代 EKF'), DeclareLaunchArgument('camera_device', default_value='/dev/video0', description='USB 摄像头设备路径'), DeclareLaunchArgument('audio_sink', default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo', description='音频输出设备 (PulseAudio sink)'), DeclareLaunchArgument('tts_speed', default_value='1.5', description='TTS 语速倍率 (0.5~2.0)'), DeclareLaunchArgument('map_file', default_value=os.path.join( planner_dir, 'maps', 'real_5x5.yaml'), description='预建地图 YAML 文件'), DeclareLaunchArgument('planner_params_file', default_value=os.path.join( planner_dir, 'config', 'real_hybrid_astar.yaml'), description='Hybrid A* 规划器参数文件'), DeclareLaunchArgument('tracker_params_file', default_value=os.path.join( planner_dir, 'config', 'real_pure_pursuit.yaml'), description='Pure Pursuit 跟踪器参数文件'), DeclareLaunchArgument('amcl_params_file', default_value=os.path.join( planner_dir, 'config', 'real_amcl.yaml'), description='AMCL 定位参数文件'), DeclareLaunchArgument('vlm_host', default_value='http://192.168.10.189:8000', description='VLM 服务器地址'), DeclareLaunchArgument('use_sim_time', default_value='false', description='使用仿真时间'), LogInfo(msg='========================================'), LogInfo(msg=' Robot System Bringup'), LogInfo(msg='========================================'), # t=0s: 底盘 + EKF TimerAction(period=0.0, actions=[ LogInfo(msg='[1/8] Starting base driver + EKF + TF + IMU ...'), origincar_bringup, ]), # t=2s: 激光雷达 TimerAction(period=2.0, actions=[ LogInfo(msg='[2/8] Starting LiDAR (lsn10) ...'), lidar_launch, ]), # t=3s: TTS 服务 + 二维码播报桥接 TimerAction(period=3.0, actions=[ LogInfo(msg='[3/8] Starting TTS Server + QR-TTS Bridge ...'), tts_server, qr_tts_bridge, ]), # t=4s: USB 摄像头 + 二维码识别 TimerAction(period=4.0, actions=[ LogInfo(msg='[4/8] Starting USB Camera + QR Detection ...'), usb_camera, qr_detect, ]), # t=5s: 障碍物检测 TimerAction(period=5.0, actions=[ LogInfo(msg='[5/8] Starting Obstacle Scanner ...'), obstacle_launch, ]), # t=7s: 地图 + AMCL 定位 TimerAction(period=7.0, actions=[ LogInfo(msg='[6/8] Starting Map Server + AMCL localization ...'), map_server_node, amcl_node, lifecycle_manager, ]), # t=9s: 路径规划 + 轨迹跟随 TimerAction(period=9.0, actions=[ LogInfo(msg='[7/8] Starting Hybrid A* Planner + Pure Pursuit Tracker ...'), planner_node, tracker_node, ]), # t=11s: VLM 图生文 TimerAction(period=11.0, actions=[ LogInfo(msg='[8/8] Starting VLM ...'), vlm_launch, ]), LogInfo(msg='========================================'), LogInfo(msg=' All modules started!'), LogInfo(msg='========================================'), ])