forked from zbw/yiliao2026
157 lines
8.0 KiB
Python
157 lines
8.0 KiB
Python
# ============================================================================
|
||
# gc_nav2_with_amcl.launch.py
|
||
# 功能：基于 AMCL（自适应蒙特卡洛定位）的导航启动文件
|
||
#
|
||
# 架构：
|
||
# Gazebo 仿真环境
|
||
# ├── robot_state_publisher — 发布机器人 TF 树（URDF 模型）
|
||
# └── nav2_bringup_launch — Nav2 一体化启动（包含以下子模块）：
|
||
# ├── map_server — 加载 my_map.yaml,发布全量静态地图到 /map
|
||
# ├── AMCL — 粒子滤波定位,发布 map→odom TF
|
||
# │ 需要手动设置 /initialpose 初始位姿
|
||
# ├── planner_server — 全局/局部路径规划器（SmacHybrid）
|
||
# ├── controller_server — 路径跟踪控制器（MPPI Ackermann）
|
||
# ├── behavior_server — 恢复行为服务器（spin/backup/wait）
|
||
# └── bt_navigator — 行为树导航编排器
|
||
#
|
||
# 与 SLAM 定位方案的区别：
|
||
# AMCL： 粒子滤波定位,需要手动设置初始位姿,地图固定不更新
|
||
# SLAM定位： 激光扫描匹配定位,自动定位,可加载 .posegraph 序列化地图
|
||
# ============================================================================
|
||
|
||
import os
|
||
from ament_index_python.packages import get_package_share_directory
|
||
from launch import LaunchDescription
|
||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
|
||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||
from launch.substitutions import LaunchConfiguration
|
||
from launch_ros.actions import Node
|
||
from launch_ros.parameter_descriptions import ParameterValue
|
||
from launch.substitutions import Command
|
||
from launch.actions import ExecuteProcess
|
||
|
||
|
||
def generate_launch_description():
|
||
"""生成 LaunchDescription,启动 Gazebo + AMCL定位 + Nav2 导航"""
|
||
ld = LaunchDescription()
|
||
|
||
# =============================1.定位到包的地址=============================================================
|
||
# 通过 ament_index 获取各功能包的安装路径
|
||
gc_navigation_fish_dir = get_package_share_directory(
|
||
'gc_navigation2_slamtoolbox')
|
||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||
origincar_urdf_dir = get_package_share_directory(
|
||
'origincar_description')
|
||
|
||
# =============================2.声明参数,获取配置文件路径===================================================
|
||
# use_sim_time: 仿真时间开关。Gazebo 下必须为 True,
|
||
# 因为仿真环境通过 /clock 话题提供时间,而非系统时间
|
||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||
|
||
# map_yaml_path: 全量静态地图 yaml 文件路径
|
||
# 传给 bringup_launch → map_server 加载并发布到 /map
|
||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
|
||
|
||
# nav2_param_path: Nav2 导航栈参数文件（含 AMCL 粒子滤波配置）
|
||
# 注意：此处用 gc_navigation_amcl.yaml,包含 AMCL 参数（粒子数、运动模型等）
|
||
# SLAM 方案则用 gc_navigation_slam.yaml（不含 AMCL）
|
||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||
gc_navigation_fish_dir, 'params', 'gc_navigation_amcl.yaml'))
|
||
|
||
# =============================3.声明启动launch文件==========================================================
|
||
# bringup_launch.py 是 Nav2 的一体化启动入口,内部自动处理：
|
||
# 1. 声明所有 launch 参数（map, slam, use_sim_time, params_file 等）
|
||
# 2. 根据 slam 参数决定启动模式：
|
||
# slam=False（默认）→ localization_launch（map_server + AMCL）
|
||
# slam=True → slam_launch（slam_toolbox 在线建图）
|
||
# 3. 启动 navigation_launch（planner + controller + behavior + bt_navigator）
|
||
# 4. 通过 RewrittenYaml 实现参数文件中的变量替换
|
||
#
|
||
# 传入参数：
|
||
# map: 地图 yaml 文件路径（map_server 加载,AMCL 定位）
|
||
# use_sim_time: 仿真时间模式（Gazebo 下必须为 true）
|
||
# params_file: Nav2 全部节点的参数配置
|
||
nav2_bringup_launch = IncludeLaunchDescription(
|
||
PythonLaunchDescriptionSource(
|
||
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
|
||
launch_arguments={
|
||
'map': map_yaml_path,
|
||
'use_sim_time': use_sim_time,
|
||
'params_file': nav2_param_path}.items(),
|
||
)
|
||
|
||
# =============================4.Gazebo仿真设置==========================================================
|
||
# robot_name_in_model: Gazebo 中机器人的模型名称,需与 URDF 中一致
|
||
robot_name_in_model = 'mycar'
|
||
|
||
# default_model_path: 机器人 URDF 模型文件路径
|
||
# 使用 xacro 宏展开生成最终的 URDF
|
||
default_model_path = os.path.join(
|
||
origincar_urdf_dir, "urdf", "origincar.urdf")
|
||
|
||
# 声明 model 启动参数,支持命令行覆盖：ros2 launch ... model:=/path/to/custom.urdf
|
||
model = DeclareLaunchArgument(
|
||
name="model", default_value=default_model_path)
|
||
|
||
# Gazebo 仿真世界文件路径
|
||
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
|
||
|
||
# 启动 Gazebo 仿真器进程
|
||
# --verbose: 输出详细日志
|
||
# -s libgazebo_ros_init.so: 加载 ROS ↔ Gazebo 通信初始化插件
|
||
# -s libgazebo_ros_factory.so: 加载模型生成（spawn_entity）插件
|
||
start_gazebo_cmd = ExecuteProcess(
|
||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
||
'-s', 'libgazebo_ros_factory.so', gazebo_world_path],
|
||
output='screen')
|
||
|
||
# 在 Gazebo 世界中生成机器人模型
|
||
# -entity: 模型在 Gazebo 中的实例名（mycar）
|
||
# -file: 要加载的 URDF 模型文件
|
||
# 生成后 Gazebo 会为模型创建对应的关节状态话题等
|
||
spawn_entity_cmd = Node(
|
||
package='gazebo_ros',
|
||
executable='spawn_entity.py',
|
||
arguments=['-entity', robot_name_in_model,
|
||
'-file', default_model_path],
|
||
output='screen'
|
||
)
|
||
|
||
# robot_description: 将 XACRO/URDF 模型字符串设为 ROS 参数
|
||
# robot_state_publisher 读取此参数发布各连杆的 TF 坐标变换
|
||
robot_description = ParameterValue(
|
||
Command(["xacro ", LaunchConfiguration("model")]), value_type=str)
|
||
|
||
# robot_state_publisher: 发布机器人各关节、连杆的 TF 变换
|
||
# - 订阅 joint_states 话题获取关节角度
|
||
# - 根据 URDF 模型计算 base_link → laser_link, wheel_link 等变换
|
||
# - publish_frequency=30Hz 确保 TF 更新平滑
|
||
# - use_sim_time=True 使用 /clock 仿真时间
|
||
robot_state_publisher = Node(
|
||
package="robot_state_publisher",
|
||
executable="robot_state_publisher",
|
||
parameters=[{"robot_description": robot_description,
|
||
'use_sim_time': True, 'publish_frequency': 30.0}]
|
||
)
|
||
|
||
# joint_state_publisher: 发布非固定关节的默认状态
|
||
# 如果 Gazebo 已经发布 joint_states,此节点可以注释掉避免冲突
|
||
# joint_state_publisher = Node(
|
||
# package="joint_state_publisher",
|
||
# executable="joint_state_publisher"
|
||
# )
|
||
|
||
# =============================5.将所有 Action 添加到 LaunchDescription ==================================
|
||
# 注意：nav2_bringup_launch 必须在 Gazebo 启动之后,
|
||
# 否则会出现 /clock 话题未就绪的问题
|
||
# 可通过 TimerAction 添加延迟,或依赖 launch 系统的自动排序
|
||
ld.add_action(model) # 1. 声明模型路径参数
|
||
ld.add_action(nav2_bringup_launch) # 2. 启动 Nav2 一体化（map_server + AMCL + 导航）
|
||
ld.add_action(start_gazebo_cmd) # 3. 启动 Gazebo 仿真器
|
||
ld.add_action(spawn_entity_cmd) # 4. 在 Gazebo 中生成机器人
|
||
ld.add_action(robot_state_publisher) # 5. 发布机器人 TF 树
|
||
# ld.add_action(joint_state_publisher) # 6. (可选) 默认关节状态发布
|
||
|
||
return ld
|