目前正在尝试仿真中实现基础地图上实时更新地图
This commit is contained in:
陌欢醉
@@ -1,79 +0,0 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_footprint
|
||||
scan_topic: /scan
|
||||
use_map_saver: true
|
||||
mode: localization #localization
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
||||
# map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
restamp_tf: false
|
||||
min_laser_range: 0.0 #for rastering images
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.0
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
min_pass_through: 2
|
||||
occupancy_threshold: 0.1
|
||||
@@ -0,0 +1,82 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# ============================ Solver 插件参数 ======================================
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ============================ ROS 基础参数 =========================================
|
||||
odom_frame: odom # 里程计坐标系
|
||||
map_frame: map # 地图坐标系
|
||||
base_frame: base_link # 机器人基座坐标系
|
||||
scan_topic: /scan # 激光雷达话题
|
||||
use_map_saver: true # 启用地图保存功能
|
||||
mode: mapping # mapping = 定位 + 实时建图（地图可更新）
|
||||
|
||||
# ============================ 已有地图加载（在其基础上继续更新）==========================
|
||||
# 启动时暂不加载（注释掉），通过服务在启动后手动加载：
|
||||
# ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph', match_type: 1}"
|
||||
# map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
|
||||
map_start_pose: [0.0, 0.0, 0.0] # 初始位姿 [x, y, yaw]（map 坐标系下）
|
||||
# map_start_at_dock: true # 或从 docking 位姿启动（与 pose 互斥）
|
||||
|
||||
# ============================ 调试与性能 ===========================================
|
||||
debug_logging: false # 调试日志（生产环境关闭）
|
||||
throttle_scans: 1 # 每 N 帧激光处理一次（1=全部处理）
|
||||
transform_publish_period: 0.02 # TF 发布周期（秒），0=不发布里程计
|
||||
map_update_interval: 3.0 # /map 话题更新间隔（秒），越小越实时
|
||||
resolution: 0.05 # 地图分辨率（米/像素）
|
||||
restamp_tf: false # 是否重新打时间戳
|
||||
min_laser_range: 0.0 # 激光最小有效距离（米）
|
||||
max_laser_range: 20.0 # 激光最大有效距离（米）
|
||||
minimum_time_interval: 0.5 # 最小处理时间间隔（秒）
|
||||
transform_timeout: 0.2 # TF 查找超时（秒）
|
||||
tf_buffer_duration: 30.0 # TF 缓冲区时长（秒）
|
||||
stack_size_to_use: 40000000 # 栈大小（序列化大地图需要）
|
||||
enable_interactive_mode: true # 交互模式：允许外部工具编辑地图
|
||||
|
||||
# ============================ 通用建图参数 =========================================
|
||||
use_scan_matching: true # 启用扫描匹配
|
||||
use_scan_barycenter: true # 使用扫描重心
|
||||
minimum_travel_distance: 0.5 # 最小移动距离触发处理（米）
|
||||
minimum_travel_heading: 0.5 # 最小转向角度触发处理（弧度）
|
||||
check_min_dist_and_heading_precisely: false
|
||||
scan_buffer_size: 10 # 扫描缓冲区大小（越大匹配越稳定）
|
||||
scan_buffer_maximum_scan_distance: 10.0 # 缓冲区最大扫描距离
|
||||
link_match_minimum_response_fine: 0.1 # 精细匹配最小响应
|
||||
link_scan_maximum_distance: 1.5 # 链接扫描最大距离
|
||||
|
||||
# ============================ 回环检测参数 =========================================
|
||||
do_loop_closing: true # 启用回环检测（消除累积误差）
|
||||
loop_match_minimum_chain_size: 10 # 最小回环链大小
|
||||
loop_match_maximum_variance_coarse: 3.0 # 粗匹配最大方差
|
||||
loop_match_minimum_response_coarse: 0.35 # 粗匹配最小响应
|
||||
loop_match_minimum_response_fine: 0.45 # 精细匹配最小响应
|
||||
loop_search_maximum_distance: 3.0 # 回环搜索最大距离
|
||||
|
||||
# ============================ 相关性匹配 - 扫描匹配参数 ==============================
|
||||
correlation_search_space_dimension: 0.5 # 搜索空间维度
|
||||
correlation_search_space_resolution: 0.01 # 搜索空间分辨率
|
||||
correlation_search_space_smear_deviation: 0.1 # 搜索空间平滑偏差
|
||||
|
||||
# ============================ 相关性匹配 - 回环检测参数 ==============================
|
||||
loop_search_space_dimension: 8.0 # 回环搜索空间维度
|
||||
loop_search_space_resolution: 0.05 # 回环搜索分辨率
|
||||
loop_search_space_smear_deviation: 0.03 # 回环搜索平滑偏差
|
||||
|
||||
# ============================ 扫描匹配器参数 ========================================
|
||||
distance_variance_penalty: 0.5 # 距离方差惩罚
|
||||
angle_variance_penalty: 1.0 # 角度方差惩罚
|
||||
|
||||
fine_search_angle_offset: 0.00349 # 精细搜索角度偏移
|
||||
coarse_search_angle_offset: 0.349 # 粗搜索角度偏移
|
||||
coarse_angle_resolution: 0.0349 # 粗搜索角度分辨率
|
||||
minimum_angle_penalty: 0.9 # 最小角度惩罚
|
||||
minimum_distance_penalty: 0.5 # 最小距离惩罚
|
||||
use_response_expansion: true # 使用响应扩展
|
||||
min_pass_through: 2 # 最小通过次数
|
||||
occupancy_threshold: 0.1 # 占据阈值
|
||||
@@ -1,137 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 154
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
- Class: slam_toolbox::SlamToolboxPlugin
|
||||
Name: SlamToolboxPlugin
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 10
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000217000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000125000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000168000001850000018500ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000017e000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
SlamToolboxPlugin:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 72
|
||||
Y: 60
|
||||
@@ -1,3 +1,24 @@
|
||||
# ============================================================================
|
||||
# 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
|
||||
@@ -11,24 +32,46 @@ 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 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
|
||||
# =============================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文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
|
||||
# =============================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']),
|
||||
@@ -38,20 +81,35 @@ def generate_launch_description():
|
||||
'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',
|
||||
@@ -60,9 +118,16 @@ def generate_launch_description():
|
||||
output='screen'
|
||||
)
|
||||
|
||||
robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
|
||||
value_type=str)
|
||||
# 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",
|
||||
@@ -70,17 +135,22 @@ def generate_launch_description():
|
||||
'use_sim_time': True, 'publish_frequency': 30.0}]
|
||||
)
|
||||
|
||||
# 2.启动 joint_state_publisher 节点发布非固定关节状态
|
||||
joint_state_publisher = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher"
|
||||
)
|
||||
# joint_state_publisher: 发布非固定关节的默认状态
|
||||
# 如果 Gazebo 已经发布 joint_states,此节点可以注释掉避免冲突
|
||||
# joint_state_publisher = Node(
|
||||
# package="joint_state_publisher",
|
||||
# executable="joint_state_publisher"
|
||||
# )
|
||||
|
||||
ld.add_action(model)
|
||||
ld.add_action(nav2_bringup_launch)
|
||||
ld.add_action(start_gazebo_cmd)
|
||||
ld.add_action(spawn_entity_cmd)
|
||||
ld.add_action(robot_state_publisher)
|
||||
# ld.add_action(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
|
||||
|
||||
@@ -1,3 +1,22 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_slam.launch.py
|
||||
# 功能：基于 slam_toolbox 定位的导航启动文件
|
||||
#
|
||||
# 架构：
|
||||
# Gazebo 仿真环境
|
||||
# ├── robot_state_publisher — 发布机器人 TF 树（URDF 模型）
|
||||
# ├── map_server — 加载 my_map.yaml，发布全量静态地图到 /map
|
||||
# ├── slam_toolbox — 加载 my_map.posegraph，激光扫描匹配定位
|
||||
# │ 发布 map→odom TF，替代 AMCL 的粒子滤波
|
||||
# │ ⚠️ /map 话题重映射为 /slam_map，避免冲突
|
||||
# └── navigation_launch — Nav2 导航栈（路径规划 + 运动控制）
|
||||
# global_costmap 从 /map 获取全量静态地图
|
||||
#
|
||||
# 与 AMCL 方案的区别：
|
||||
# AMCL： 粒子滤波定位，需要手动设置初始位姿
|
||||
# SLAM定位： 激光扫描匹配定位，自动根据地图特征定位
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
@@ -11,27 +30,50 @@ from launch.actions import ExecuteProcess
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""生成 LaunchDescription，启动 Gazebo + SLAM定位 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
# 通过 ament_index 获取各功能包的安装路径
|
||||
pkg_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 文件路径
|
||||
# 由 map_server 加载，发布到 /map 供 global_costmap 使用
|
||||
map_yaml_path = LaunchConfiguration('map', default=os.path.join(
|
||||
pkg_dir, 'maps', 'my_map.yaml'))
|
||||
|
||||
# nav2_param_path: Nav2 导航栈参数文件（不含 AMCL，含 slam_toolbox）
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam.yaml'))
|
||||
|
||||
# slam_params_file: slam_toolbox 定位模式的参数文件
|
||||
# mode: localization — 只定位不建图
|
||||
# 包含 solver 配置、激光匹配参数、TF 帧定义等
|
||||
slam_params_file = os.path.join(
|
||||
pkg_dir, 'config', 'mapper_params_localization.yaml')
|
||||
# slam_toolbox 定位所需的序列化地图
|
||||
|
||||
# posegraph_path: slam_toolbox 定位所需的序列化地图文件
|
||||
# .posegraph 文件保存了 slam_toolbox 建图时的位姿图
|
||||
# 配合 .data 文件，slam_toolbox 可以还原完整地图用于激光匹配
|
||||
posegraph_path = os.path.join(
|
||||
pkg_dir, 'maps', 'my_map.posegraph')
|
||||
|
||||
# =============================3.slam_toolbox 定位模式（替代 AMCL）=========================================
|
||||
# 注意：slam_toolbox 的 /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
|
||||
# localization_slam_toolbox_node:
|
||||
# - 加载 .posegraph 序列化地图用于激光扫描匹配
|
||||
# - 发布 map→odom 坐标变换（替代 AMCL 的粒子滤波定位）
|
||||
# - 发布 /pose 话题（当前定位位姿）
|
||||
#
|
||||
# ⚠️ 关键：将 slam_toolbox 的 /map 重映射为 /slam_map
|
||||
# 避免覆盖 map_server 发布到 /map 的全量静态地图
|
||||
# 确保 global_costmap 的 static_layer 能获取完整地图边界
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='localization_slam_toolbox_node',
|
||||
@@ -44,6 +86,11 @@ def generate_launch_description():
|
||||
)
|
||||
|
||||
# =============================4.map_server 提供全量静态地图给 global_costmap =============================
|
||||
# map_server:
|
||||
# - 加载 my_map.yaml + my_map.pgm 格式的传统静态地图
|
||||
# - 发布完整地图到 /map 话题（199×266 像素，分辨率 0.05 m）
|
||||
# - 这是 global_costmap 静态层的唯一数据来源
|
||||
# - 是 lifecycle 节点，需要 lifecycle_manager 激活
|
||||
map_server_node = Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
@@ -53,6 +100,8 @@ def generate_launch_description():
|
||||
'use_sim_time': True}],
|
||||
)
|
||||
|
||||
# lifecycle_manager: 管理 map_server 的生命周期（configure → activate）
|
||||
# autostart=True 表示启动后自动激活 map_server
|
||||
map_server_lifecycle = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
@@ -64,7 +113,14 @@ def generate_launch_description():
|
||||
)
|
||||
|
||||
# =============================5.仅启动 nav2 navigation（不含 localization）=================================
|
||||
# 因为 slam_toolbox 已经替代了 AMCL，不需要再跑 localization_launch
|
||||
# 为什么用 navigation_launch 而不是 bringup_launch？
|
||||
# bringup_launch 包含 localization_launch（AMCL + map_server）
|
||||
# 由于 slam_toolbox 已经替代了 AMCL，只需 navigation_launch 的：
|
||||
# - planner_server — 全局/局部路径规划
|
||||
# - controller_server — 路径跟踪控制（MPPI）
|
||||
# - behavior_server — 行为树（spin/backup/wait 恢复）
|
||||
# - bt_navigator — 行为树导航编排
|
||||
# - costmap 层 — global_costmap + local_costmap
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
@@ -73,19 +129,34 @@ def generate_launch_description():
|
||||
'params_file': nav2_param_path}.items(),
|
||||
)
|
||||
|
||||
# =============================5.Gazebo仿真设置==========================================================
|
||||
# =============================6.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 启动参数，支持命令行覆盖 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: 加载模型生成插件
|
||||
start_gazebo_cmd = ExecuteProcess(
|
||||
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
||||
'-s', 'libgazebo_ros_factory.so', gazebo_world_path],
|
||||
output='screen')
|
||||
|
||||
# 在 Gazebo 中生成机器人模型
|
||||
# -entity: 模型实例名称
|
||||
# -file: URDF 模型文件
|
||||
spawn_entity_cmd = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
@@ -94,9 +165,14 @@ def generate_launch_description():
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# robot_description: 将 URDF/XACRO 模型转换为 ROS 参数
|
||||
# robot_state_publisher 订阅此参数发布 TF 树
|
||||
robot_description = ParameterValue(
|
||||
Command(["xacro ", LaunchConfiguration("model")]), value_type=str)
|
||||
|
||||
# robot_state_publisher: 发布机器人各连杆的 TF 变换
|
||||
# base_link → laser_link, wheel_link 等关节状态
|
||||
# publish_frequency=30Hz 保证平滑的 TF 更新
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
@@ -104,13 +180,17 @@ def generate_launch_description():
|
||||
'use_sim_time': True, 'publish_frequency': 30.0}]
|
||||
)
|
||||
|
||||
ld.add_action(model)
|
||||
ld.add_action(start_gazebo_cmd)
|
||||
ld.add_action(spawn_entity_cmd)
|
||||
ld.add_action(robot_state_publisher)
|
||||
ld.add_action(map_server_node)
|
||||
ld.add_action(map_server_lifecycle)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
ld.add_action(navigation_launch)
|
||||
# =============================7.将所有 Action 添加到 LaunchDescription ==================================
|
||||
# 启动顺序由 Launch 系统自动管理依赖
|
||||
ld.add_action(model) # 1. 声明模型参数
|
||||
ld.add_action(start_gazebo_cmd) # 2. 启动 Gazebo 仿真器
|
||||
ld.add_action(spawn_entity_cmd) # 3. 生成机器人模型
|
||||
ld.add_action(robot_state_publisher) # 4. 发布 TF 树
|
||||
ld.add_action(map_server_node) # 5. 加载全量静态地图 → /map
|
||||
ld.add_action(map_server_lifecycle) # 6. 激活 map_server
|
||||
ld.add_action(slam_toolbox_node) # 7. 启动 SLAM 定位 → map→odom TF
|
||||
ld.add_action(navigation_launch) # 8. 启动 Nav2 导航栈
|
||||
|
||||
return ld
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,158 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_slam_online.launch.py
|
||||
# 功能：基于 slam_toolbox 在线异步建图 + 定位的导航启动文件
|
||||
#
|
||||
# 核心能力：
|
||||
# 1. 启动时加载已有地图（my_map.posegraph），无需从零建图
|
||||
# 2. 运行中持续更新地图（新障碍物自动写入 /map）
|
||||
# 3. 激光扫描匹配定位（替代 AMCL，自动定位）
|
||||
# 4. 双层避障：obstacle_layer（/scan 瞬时避障）+ static_layer（/map 持久化障碍）
|
||||
#
|
||||
# 架构：
|
||||
# Gazebo 仿真环境
|
||||
# ├── robot_state_publisher — URDF → TF 树（base_link→laser_link 等）
|
||||
# ├── slam_toolbox — async_slam_toolbox_node
|
||||
# │ ├── 加载 my_map.posegraph + my_map.data
|
||||
# │ ├── mode: mapping（定位 + 实时建图）
|
||||
# │ ├── 发布 /map（全量 + 实时更新）
|
||||
# │ └── 发布 map→odom TF（定位结果）
|
||||
# └── navigation_launch — Nav2 导航栈
|
||||
# ├── global_costmap ← /map（静态层）
|
||||
# ├── local_costmap ← /scan（障碍层）
|
||||
# ├── planner_server — SmacHybrid 路径规划
|
||||
# ├── controller_server — MPPI Ackermann 控制
|
||||
# ├── behavior_server — spin/backup/wait 恢复
|
||||
# └── bt_navigator — 行为树编排
|
||||
#
|
||||
# 与其他方案对比：
|
||||
# gc_nav2_with_amcl.launch.py: AMCL 粒子滤波，地图固定，需手动设初始位姿
|
||||
# gc_nav2_with_slam.launch.py: SLAM 定位模式，地图只读，需 map_server
|
||||
# gc_nav2_with_slam_online.launch.py: SLAM 在线模式，加载已有地图 + 实时更新（本文件）
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
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():
|
||||
"""启动 Gazebo + SLAM 在线建图定位 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
pkg_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: 必须为 true，Gazebo 通过 /clock 话题提供仿真时间
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
|
||||
# nav2_param_path: Nav2 导航栈参数（planner/controller/costmap/behavior）
|
||||
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
|
||||
pkg_dir, 'params', 'gc_navigation_slam.yaml'))
|
||||
|
||||
# slam 基础配置文件（异步建图 + 实时更新）
|
||||
slam_params_file = os.path.join(
|
||||
pkg_dir, 'config', 'slam_toolbox_async.yaml')
|
||||
|
||||
# 已有序列化地图（slam_toolbox 格式：.posegraph + .data）
|
||||
posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
|
||||
|
||||
# =============================3.slam_toolbox：在线异步建图 + 定位============================================
|
||||
# 选用 online_async_slam_toolbox_node（而非 localization_slam_toolbox_node）的原因：
|
||||
# - 加载已有 .posegraph 地图后，仍可继续写入新扫描数据
|
||||
# - 异步处理：激光数据在独立线程中建图，不阻塞里程计
|
||||
# - 适合导航场景：启动时还原已知区域，运行中发现新障碍物自动更新 /map
|
||||
#
|
||||
# 参数说明（以下参数已在 mapper_params_online_async.yaml 中配置，此处覆写确保生效）：
|
||||
# mode: mapping — 定位 + 建图（覆盖 yaml）
|
||||
# map_file_name — 已在 yaml 中设为源码路径，不在此覆写
|
||||
|
||||
|
||||
# 已有序列化地图（slam_toolbox 格式：.posegraph + .data）
|
||||
# 注意：此路径仅供文档参考，实际加载路径在 yaml 配置文件中
|
||||
posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
|
||||
slam_toolbox_node = Node(
|
||||
package='slam_toolbox',
|
||||
executable='async_slam_toolbox_node',
|
||||
name='slam_toolbox',
|
||||
output='screen',
|
||||
parameters=[
|
||||
slam_params_file, # 基础参数（solver、TF、分辨率、map_file_name 等）
|
||||
{
|
||||
'use_sim_time': True,
|
||||
'mode': 'mapping', # 覆盖 yaml：定位 + 建图
|
||||
'map_update_interval': 3.0, # 每 3 秒更新一次 /map
|
||||
},
|
||||
],
|
||||
# 无需 remap /map：online_async 发布的就是权威 /map
|
||||
)
|
||||
# Read: Failed to open requested file: / home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph.
|
||||
# Failed to read file: / home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph.
|
||||
# =============================4.启动 Nav2 导航栈==========================================================
|
||||
# 只用 navigation_launch（不含 localization），因为 slam_toolbox 已承担定位
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# =============================5.Gazebo 仿真环境==========================================================
|
||||
robot_name_in_model = 'mycar'
|
||||
default_model_path = os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf')
|
||||
model = DeclareLaunchArgument(name='model', default_value=default_model_path)
|
||||
|
||||
# 启动 Gazebo 仿真器（加载 world 文件和 ROS 插件）
|
||||
gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'test.world')
|
||||
start_gazebo_cmd = ExecuteProcess(
|
||||
cmd=[
|
||||
'gazebo', '--verbose',
|
||||
'-s', 'libgazebo_ros_init.so', # ROS ↔ Gazebo 通信初始化
|
||||
'-s', 'libgazebo_ros_factory.so', # 模型生成工厂
|
||||
gazebo_world_path,
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# 在 Gazebo 中生成机器人
|
||||
spawn_entity_cmd = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-entity', robot_name_in_model, '-file', default_model_path],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# 发布机器人 TF 树
|
||||
robot_description = ParameterValue(
|
||||
Command(['xacro ', LaunchConfiguration('model')]), value_type=str,
|
||||
)
|
||||
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,
|
||||
}],
|
||||
)
|
||||
|
||||
# =============================6.组装 LaunchDescription===================================================
|
||||
ld.add_action(model)
|
||||
ld.add_action(start_gazebo_cmd)
|
||||
ld.add_action(spawn_entity_cmd)
|
||||
ld.add_action(robot_state_publisher)
|
||||
ld.add_action(slam_toolbox_node)
|
||||
ld.add_action(navigation_launch)
|
||||
|
||||
return ld
|
||||
Reference in New Issue
Block a user