353 lines
14 KiB
Python
Executable File
353 lines
14 KiB
Python
Executable File
#!/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='========================================'),
|
||
])
|