From f95b08714310ed704b22c745ede90adf0bc59610 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=99=8C=E6=AC=A2=E9=86=89?= <264854363@qq.com> Date: Thu, 4 Jun 2026 22:42:10 +0800 Subject: [PATCH] =?UTF-8?q?add=EF=BC=9A=E5=9C=A8=E5=9F=BA=E7=A1=80?= =?UTF-8?q?=E5=9C=B0=E5=9B=BE=E4=B8=8A=E5=AE=9E=E7=8E=B0=E4=BA=86=E5=AE=9E?= =?UTF-8?q?=E6=97=B6=E9=81=BF=E9=9A=9C=E4=BB=A5=E5=8F=8A=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=E4=BA=86=E6=AF=94=E8=BE=83=E7=81=B5=E6=B4=BB=E7=9A=84=E5=90=8E?= =?UTF-8?q?=E6=92=A4=EF=BC=8C=E4=BB=8E=E8=80=8C=E5=AE=9E=E7=8E=B0=E6=8E=89?= =?UTF-8?q?=E5=A4=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/nav_to_pose_ackermann.xml | 36 +++++ .../launch/gc_nav2_with_slam_online.launch.py | 152 ++++++++---------- .../params/gc_navigation_slam.yaml | 8 +- 3 files changed, 112 insertions(+), 84 deletions(-) create mode 100644 src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml diff --git a/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml b/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml new file mode 100644 index 0000000..3cda7ff --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py index d4c760a..6c5b0d1 100644 --- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py @@ -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 diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml index cb4c1df..47b13f3 100644 --- a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml +++ b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml @@ -10,6 +10,8 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 20 default_server_timeout: 20 + # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待 + default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_compute_path_through_poses_action_bt_node @@ -269,7 +271,7 @@ planner_server: analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 minimum_turning_radius: 0.40 - reverse_penalty: 2.0 + reverse_penalty: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车） change_penalty: 0.0 non_straight_penalty: 1.2 cost_penalty: 2.0 @@ -308,11 +310,13 @@ behavior_server: cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors/Spin" # 需保留以匹配默认 BT XML backup: plugin: "nav2_behaviors/BackUp" + backup_dist: 0.5 wait: plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 global_frame: odom robot_base_frame: base_link transform_tolerance: 0.5