add：在基础地图上实现了实时避障以及实现了比较灵活的后撤，从而实现掉头
This commit is contained in:
陌欢醉
@@ -1,33 +1,25 @@
|
||||
# ============================================================================
|
||||
# gc_nav2_with_slam_online.launch.py
|
||||
# 功能：基于 slam_toolbox 在线异步建图 + 定位的导航启动文件
|
||||
#
|
||||
# 核心能力：
|
||||
# 1. 启动时加载已有地图（my_map.posegraph），无需从零建图
|
||||
# 2. 运行中持续更新地图（新障碍物自动写入 /map）
|
||||
# 3. 激光扫描匹配定位（替代 AMCL，自动定位）
|
||||
# 4. 双层避障：obstacle_layer（/scan 瞬时避障）+ static_layer（/map 持久化障碍）
|
||||
# 功能：已知全局地图 + SLAM 在线更新 + 导航
|
||||
#
|
||||
# 架构：
|
||||
# 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 — 行为树编排
|
||||
# ┌─ /map（全量静态地图，199×266）
|
||||
# map_server ──────────┤
|
||||
# └─ 来自 my_map.yaml + my_map.pgm
|
||||
#
|
||||
# 与其他方案对比：
|
||||
# gc_nav2_with_amcl.launch.py: AMCL 粒子滤波，地图固定，需手动设初始位姿
|
||||
# gc_nav2_with_slam.launch.py: SLAM 定位模式，地图只读，需 map_server
|
||||
# gc_nav2_with_slam_online.launch.py: SLAM 在线模式，加载已有地图 + 实时更新（本文件）
|
||||
# ┌─ map→odom TF（激光扫描匹配定位）
|
||||
# slam_toolbox ────────┤
|
||||
# (async mapping) └─ /slam_map（实时建图结果，重映射避免冲突）
|
||||
#
|
||||
# navigation ┌─ global_costmap/static_layer ← /map（全量地图）
|
||||
# ├─ local_costmap/obstacle_layer ← /scan（实时避障）
|
||||
# └─ planner + controller + behavior
|
||||
#
|
||||
# 关键设计：
|
||||
# 1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
|
||||
# 2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
|
||||
# 3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
|
||||
# 4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
|
||||
# ============================================================================
|
||||
|
||||
import os
|
||||
@@ -43,62 +35,66 @@ from launch.actions import ExecuteProcess
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""启动 Gazebo + SLAM 在线建图定位 + Nav2 导航"""
|
||||
"""启动 Gazebo + 已知全局地图 + SLAM 在线定位建图 + Nav2 导航"""
|
||||
ld = LaunchDescription()
|
||||
|
||||
# =============================1.定位到包的地址=============================================================
|
||||
# =============================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 话题提供仿真时间
|
||||
# =============================2.参数==================================================================
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
|
||||
# nav2_param_path: Nav2 导航栈参数（planner/controller/costmap/behavior）
|
||||
map_yaml_path = os.path.join(pkg_dir, 'maps', 'my_map.yaml')
|
||||
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）
|
||||
slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml')
|
||||
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 中设为源码路径，不在此覆写
|
||||
# =============================3.map_server：全量静态地图（Foxglove 看到的全局地图）========================
|
||||
map_server_node = Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': True}],
|
||||
)
|
||||
map_server_lifecycle = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_map',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': True, 'autostart': True,
|
||||
'node_names': ['map_server']}],
|
||||
)
|
||||
|
||||
|
||||
# 已有序列化地图（slam_toolbox 格式：.posegraph + .data）
|
||||
# 注意：此路径仅供文档参考，实际加载路径在 yaml 配置文件中
|
||||
posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
|
||||
# =============================4.slam_toolbox：定位 + 实时建图=============================================
|
||||
# ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
|
||||
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
|
||||
parameters=[slam_params_file, {'use_sim_time': True}],
|
||||
remappings=[('/map', '/slam_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 已承担定位
|
||||
|
||||
# =============================5.加载已有序列化地图=======================================================
|
||||
# 通过 ros2 service call 在 slam_toolbox 启动后加载
|
||||
import launch
|
||||
load_map_cmd = ExecuteProcess(
|
||||
cmd=[
|
||||
'bash', '-c',
|
||||
# 等待 slam_toolbox 服务就绪后加载地图
|
||||
'sleep 8 && '
|
||||
'ros2 service call /slam_toolbox/deserialize_map '
|
||||
'slam_toolbox/srv/DeserializePoseGraph '
|
||||
"'{filename: \"%s\", match_type: 1}'" % posegraph_path,
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# =============================6.Nav2 导航栈============================================================
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||
@@ -108,51 +104,43 @@ def generate_launch_description():
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# =============================5.Gazebo 仿真环境==========================================================
|
||||
# =============================7.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,
|
||||
],
|
||||
cmd=['gazebo', '--verbose',
|
||||
'-s', 'libgazebo_ros_init.so',
|
||||
'-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,
|
||||
}],
|
||||
parameters=[{'robot_description': robot_description,
|
||||
'use_sim_time': True, 'publish_frequency': 30.0}],
|
||||
)
|
||||
|
||||
# =============================6.组装 LaunchDescription===================================================
|
||||
# =============================8.组装==================================================================
|
||||
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(load_map_cmd)
|
||||
ld.add_action(navigation_launch)
|
||||
|
||||
return ld
|
||||
|
||||
Reference in New Issue
Block a user