迁移了原来的nav2包；新增基于obstacle_scanner构建costmap的nav2，暂未实际测试

src/navigation/gc_navigation2_slamtoolbox/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(gc_navigation2_slamtoolbox)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+# 查找slam_toolbox包
+find_package(slam_toolbox REQUIRED)
+install(
+  DIRECTORY launch config params maps world
+  DESTINATION share/${PROJECT_NAME}
+)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_lifelong.yaml

@@ -0,0 +1,89 @@
+
+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_link
+    scan_topic: /scan
+    use_map_saver: true
+    mode: mapping
+
+    # lifelong params
+    lifelong_search_use_tree: false
+    lifelong_minimum_score: 0.1
+    lifelong_iou_match: 0.85
+    lifelong_node_removal_score: 0.04
+    lifelong_overlap_score_scale: 0.06
+    lifelong_constraint_multiplier: 0.08
+    lifelong_nearby_penalty: 0.001
+    lifelong_candidates_scale: 0.03
+
+    # 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: test_steve
+    #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: 10.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+
+    # 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

src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_localization.yaml

@@ -0,0 +1,72 @@
+slam_toolbox:
+  ros__parameters:
+    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_link
+    scan_topic: /scan
+    mode: localization #localization
+
+    # if you'd like to start localizing on bringup in a map and 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]
+
+    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.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+
+    # 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: 3
+    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: 3
+    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
+    loop_search_maximum_distance: 3.0
+
+    # 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

src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_offline.yaml

@@ -0,0 +1,71 @@
+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: mapping #localization
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 10.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: 14400.
+    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

src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_online_multi_async.yaml

@@ -0,0 +1,74 @@
+$(var namespace)/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: global_odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: scan
+    mode: mapping #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: test_steve
+    # 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
+    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

src/navigation/gc_navigation2_slamtoolbox/config/mapper_params_online_sync.yaml

@@ -0,0 +1,79 @@
+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: mapping #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: test_steve
+    #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.
+    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

src/navigation/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml

@@ -0,0 +1,29 @@
+<!--
+  Ackermann navigate-through-poses behavior tree.
+  Keeps periodic replanning while following. Recovery is intentionally short:
+  local controller clear on path-follow failure, then BackUp only at the top level.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <ReactiveSequence>
+            <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+            <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
+          </ReactiveSequence>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <BackUp backup_dist="0.80" backup_speed="0.18"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

src/navigation/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml

@@ -0,0 +1,26 @@
+<!--
+  Ackermann navigation behavior tree.
+  Keeps 1 Hz replanning while following. Recovery is intentionally short:
+  local controller clear on path-follow failure, then BackUp only at the top level.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="0.5">
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <BackUp backup_dist="0.80" backup_speed="0.18"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml

@@ -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                # 机器人基座坐标系（与 URDF 根帧一致）
+    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: [-2.0, -2.0, 0.0]           # 初始位姿 [x, y, yaw]（map 坐标系下）
+    # map_start_at_dock: true                  # 或从 docking 位姿启动（与 pose 互斥）
+
+    # ============================ 调试与性能 ===========================================
+    debug_logging: false                       # 调试日志（生产环境关闭）
+    throttle_scans: 10                          # 每 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.1               # 最小移动距离触发处理（米）
+    minimum_travel_heading: 0.1                # 最小转向角度触发处理（弧度）
+    check_min_dist_and_heading_precisely: false
+    scan_buffer_size: 5                       # 扫描缓冲区大小（越大匹配越稳定）
+    scan_buffer_maximum_scan_distance: 5.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                   # 占据阈值

src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_localization.yaml

@@ -0,0 +1,55 @@
+slam_toolbox:
+  ros__parameters:
+    # 核心模式设置
+    use_sim_time: true
+    mode: "localization"  # 重要：设置为定位模式
+    map_file_name: ""  # 由启动参数传入
+    map_start_pose: [0.0,0.0,0.0]
+    # ROS框架设置
+    odom_frame: "odom"
+    map_frame: "map"
+    base_frame: "base_footprint"
+    scan_topic: "/scan"
+    
+    # 求解器设置
+    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"
+    
+    # 定位优化参数
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.05
+    minimum_travel_heading: 0.087
+    
+    # 扫描处理
+    scan_buffer_size: 50
+    throttle_scans: 2
+    
+    # 回环检测（定位模式可关闭）
+    do_loop_closing: false
+    
+    # 搜索参数
+    link_match_minimum_response_fine: 0.2
+    link_scan_maximum_distance: 2.0
+    loop_search_maximum_distance: 5.0
+    
+    # 性能参数
+    minimum_time_interval: 0.1
+    transform_publish_period: 0.02
+    map_update_interval: 0.5
+    
+    # 地图参数
+    resolution: 0.05
+    max_laser_range: 15.0
+    min_laser_range: 0.2
+    
+    # 初始位姿（可选，可以通过RViz设置）
+    # map_start_pose: [0.0, 0.0, 0.0]
+    
+    # 调试
+    debug_logging: false
+    enable_interactive_mode: true

src/navigation/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml

@@ -0,0 +1,73 @@
+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
+    mode: mapping
+    use_map_saver: true
+
+    # === 调试与性能 ===
+    debug_logging: false
+    throttle_scans: 2
+    transform_publish_period: 0.02
+    map_update_interval: 0.8
+    resolution: 0.05
+    min_laser_range: 0.15
+    max_laser_range: 10.0
+    minimum_time_interval: 0.7
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    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.1
+    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
+
+    # MessageFilter queue size (default 1, too small for high-rate lidar)
+    scan_queue_size: 20

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py

@@ -0,0 +1,156 @@
+# ============================================================================
+# 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

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py

@@ -0,0 +1,196 @@
+# ============================================================================
+# 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
+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():
+    """生成 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')
+
+    # 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）=========================================
+    # 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',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[slam_params_file,
+                    {'use_sim_time': True,
+                     'map_file_name': posegraph_path}],
+        remappings=[('/map', '/slam_map')],
+    )
+
+    # =============================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',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': map_yaml_path,
+                     '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',
+        name='lifecycle_manager_map',
+        output='screen',
+        parameters=[{'use_sim_time': True,
+                     'autostart': True,
+                     'node_names': ['map_server']}],
+    )
+
+    # =============================5.仅启动 nav2 navigation（不含 localization）=================================
+    # 为什么用 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']),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+            'params_file': nav2_param_path}.items(),
+    )
+
+    # =============================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',
+        arguments=['-entity', robot_name_in_model,
+                   '-file', default_model_path],
+        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",
+        parameters=[{"robot_description": robot_description,
+                     'use_sim_time': True, 'publish_frequency': 30.0}]
+    )
+
+    # =============================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
+    
+

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py

@@ -0,0 +1,181 @@
+# ============================================================================
+# gc_nav2_with_slam_online.launch.py
+# 功能：已知全局地图 + SLAM 在线更新 + 导航
+#
+# 架构：
+#                        ┌─ /map（全量静态地图，199×266）
+#   map_server ──────────┤
+#                        └─ 来自 my_map.yaml + my_map.pgm
+#
+#                        ┌─ 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
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, SetEnvironmentVariable
+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 = LaunchConfiguration('use_sim_time', default='true')
+    map_yaml_path = os.path.join(pkg_dir, 'maps', 'zhihui.yaml')
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        pkg_dir, 'params', 'gc_navigation_slam.yaml'))
+    slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml')
+    posegraph_path = os.path.join(pkg_dir, 'maps', 'zhihui.posegraph')
+
+    # =============================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']}],
+    )
+
+    # =============================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, {'use_sim_time': True}],
+        remappings=[('/map', '/slam_map')],
+    )
+
+    # =============================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']),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+            'params_file': nav2_param_path,
+        }.items(),
+    )
+
+    # =============================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_path = os.path.join(origincar_urdf_dir, 'world', 'zhihui.world')
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=['gazebo', '--verbose',
+             '-s', 'libgazebo_ros_init.so',
+             '-s', 'libgazebo_ros_factory.so',
+             gazebo_world_path],
+        output='screen',
+    )
+
+    # =============================7a.机器人初始位姿参数=====================================================
+    # 通过命令行参数覆盖，例如：
+    #   ros2 launch ... spawn_x:=1.0 spawn_y:=-2.0 spawn_yaw:=1.57
+    spawn_x = LaunchConfiguration('spawn_x', default='-4.311092')
+    spawn_y = LaunchConfiguration('spawn_y', default='-4.299756')
+    spawn_z = LaunchConfiguration('spawn_z', default='0.0')
+    spawn_yaw = LaunchConfiguration('spawn_yaw', default='0.0')
+
+    set_spawn_x = SetEnvironmentVariable('SPAWN_X', spawn_x)
+    set_spawn_y = SetEnvironmentVariable('SPAWN_Y', spawn_y)
+    set_spawn_z = SetEnvironmentVariable('SPAWN_Z', spawn_z)
+    set_spawn_yaw = SetEnvironmentVariable('SPAWN_YAW', spawn_yaw)
+
+    # =============================7b.在 Gazebo 中生成机器人=================================================
+    # 通过 bash 读取环境变量，确保位姿参数正确传递
+    spawn_entity_cmd = ExecuteProcess(
+        cmd=['bash', '-c',
+             'ros2 run gazebo_ros spawn_entity.py '
+             '-entity mycar '
+             f'-file {default_model_path} '
+             '-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'],
+        output='screen',
+    )
+
+    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}],
+    )
+
+    # =============================8.组装==================================================================
+    ld.add_action(model)
+    ld.add_action(DeclareLaunchArgument(
+        'spawn_x', default_value='-4.311092',
+        description='机器人初始 x 坐标（m）'))
+    ld.add_action(DeclareLaunchArgument(
+        'spawn_y', default_value='-4.299756',
+        description='机器人初始 y 坐标（m）'))
+    ld.add_action(DeclareLaunchArgument(
+        'spawn_z', default_value='0.0',
+        description='机器人初始 z 坐标（m）'))
+    ld.add_action(DeclareLaunchArgument(
+        'spawn_yaw', default_value='0.0',
+        description='机器人初始偏航角（rad）'))
+    ld.add_action(start_gazebo_cmd)
+    ld.add_action(set_spawn_x)
+    ld.add_action(set_spawn_y)
+    ld.add_action(set_spawn_z)
+    ld.add_action(set_spawn_yaw)
+    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

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py

@@ -0,0 +1,56 @@
+# ============================================================================
+# gc_nav2_with_slam_online_real.launch.py
+# 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
+# 依赖：需先启动 origincar_bringup（底盘驱动 + EKF）
+# ============================================================================
+
+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
+
+
+def generate_launch_description():
+    """真机实时建图：slam_toolbox 从零建图 + Nav2 导航"""
+    ld = LaunchDescription()
+
+    # === 1. 包路径 ===
+    pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+    # === 2. 参数 ===
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        pkg_dir, 'params', 'gc_navigation_slam.yaml'))
+    slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')
+
+    # === 3. slam_toolbox：实时建图 → 发布 map→odom ===
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[slam_params_file,
+                     {'use_sim_time': use_sim_time}],
+    )
+
+    # === 4. Nav2 导航栈 ===
+    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. TF 树由底盘启动文件（origincar_base）提供，此处不再重复发布 ===
+
+    # === 6. 组装 ===
+    ld.add_action(slam_toolbox_node)
+    ld.add_action(navigation_launch)
+
+    return ld

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+    params_file = os.path.join(
+        pkg_share, 'config', 'mapper_params_lifelong.yaml')
+
+    # 创建SLAM Toolbox节点
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file]
+    )
+
+    return LaunchDescription([
+        slam_toolbox_node
+    ])

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+    params_file = os.path.join(
+        pkg_share, 'config', 'mapper_params_localization.yaml')
+
+    # 创建SLAM Toolbox节点
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='localization_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file]
+    )
+
+    return LaunchDescription([
+        slam_toolbox_node
+    ])

src/navigation/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+    params_file = os.path.join(
+        pkg_share, 'config', 'mapper_params_online_async.yaml')
+
+    # 创建SLAM Toolbox节点
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file]
+    )
+
+    return LaunchDescription([
+        slam_toolbox_node
+    ])

src/navigation/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py

@@ -0,0 +1,196 @@
+#!/usr/bin/env python3
+# ============================================================================
+# gc_slam_mapping.launch.py
+# 功能：Gazebo 仿真环境 + SLAM 建图启动文件
+#
+# 适用场景：
+#   - 在 Gazebo 仿真中遥控机器人建立环境地图
+#   - 在已有地图基础上增量更新
+#   - 单独调试 slam_toolbox 建图参数
+#
+# 架构：
+#   Gazebo 仿真器          — 加载 zhihui.world（智慧楼墙体环境）
+#   spawn_entity           — 在 Gazebo 中生成机器人模型
+#   robot_state_publisher  — 发布机器人 TF 树（URDF 模型）
+#   joint_state_publisher  — 发布关节状态
+#   async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式）
+#                             发布 map→odom TF
+#                             发布 /map 话题（实时地图）
+#
+# 使用方式：
+#   ros2 launch gc_navigation2_slamtoolbox gc_slam_mapping.launch.py
+#
+# 保存地图：
+#   ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
+#     "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map'}"
+#   ros2 run nav2_map_server map_saver_cli -f <map_name>
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch.substitutions import Command
+
+
+def generate_launch_description():
+    """生成 LaunchDescription，启动 Gazebo + slam_toolbox 建图"""
+
+    # ============================ 1. 包路径 =========================================
+    pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
+    origincar_urdf_dir = get_package_share_directory('origincar_description')
+
+    # ============================ 2. 声明启动参数 ====================================
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+
+    # slam_params_file: slam_toolbox 配置文件
+    slam_params_file = LaunchConfiguration(
+        'slam_params_file',
+        default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'))
+
+    # map_file_name: 可选，加载已有序列化地图继续建图
+    map_file_name = LaunchConfiguration('map_file_name', default='')
+
+    # model: 机器人 URDF 模型路径（用于 xacro 处理）
+    # origincar.urdf 是纯 URDF（不含 xacro 宏），xacro 会原样透传
+    default_model_path = os.path.join(
+        origincar_urdf_dir, 'urdf', 'origincar.urdf')
+    model = DeclareLaunchArgument(
+        name='model', default_value=default_model_path)
+
+    # spawn_model: Gazebo 中生成机器人用的 URDF（不含 xacro 宏）
+    spawn_model_path = os.path.join(
+        origincar_urdf_dir, 'urdf', 'origincar.urdf')
+
+    # ============================ 3. 设置 GAZEBO_MODEL_PATH ==========================
+    # 让 Gazebo 能找到 zhihui 模型，同时保留标准模型路径
+    gazebo_model_path = os.path.join(pkg_dir, 'world')
+    existing_gazebo_path = os.environ.get('GAZEBO_MODEL_PATH', '')
+    if existing_gazebo_path:
+        full_gazebo_path = f'{gazebo_model_path}:{existing_gazebo_path}'
+    else:
+        full_gazebo_path = gazebo_model_path
+    set_gazebo_model_path = SetEnvironmentVariable(
+        name='GAZEBO_MODEL_PATH',
+        value=full_gazebo_path
+    )
+
+    # ============================ 4. 启动 Gazebo 仿真器 ==============================
+    gazebo_world_path = os.path.join(pkg_dir, 'world', 'zhihui.world')
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=['gazebo', '--verbose',
+             '-s', 'libgazebo_ros_init.so',
+             '-s', 'libgazebo_ros_factory.so',
+             gazebo_world_path],
+        output='screen'
+    )
+
+    # ============================ 5. 机器人初始位姿参数 ==============================
+    # 通过命令行参数覆盖，例如：
+    #   ros2 launch ... spawn_x:=1.0 spawn_y:=-2.0 spawn_yaw:=1.57
+    spawn_x = LaunchConfiguration('spawn_x', default='-4.311092')
+    spawn_y = LaunchConfiguration('spawn_y', default='-4.299756')
+    spawn_z = LaunchConfiguration('spawn_z', default='0.0')
+    spawn_yaw = LaunchConfiguration('spawn_yaw', default='0.0')
+
+    # 将位姿参数设为环境变量，供 spawn 命令使用
+    set_spawn_x = SetEnvironmentVariable('SPAWN_X', spawn_x)
+    set_spawn_y = SetEnvironmentVariable('SPAWN_Y', spawn_y)
+    set_spawn_z = SetEnvironmentVariable('SPAWN_Z', spawn_z)
+    set_spawn_yaw = SetEnvironmentVariable('SPAWN_YAW', spawn_yaw)
+
+    # ============================ 6. 在 Gazebo 中生成机器人 ==========================
+    # 通过 bash 读取环境变量，确保位姿参数正确传递
+    robot_name_in_model = 'mycar'
+    spawn_entity_cmd = ExecuteProcess(
+        cmd=['bash', '-c',
+             'ros2 run gazebo_ros spawn_entity.py '
+             '-entity mycar '
+             f'-file {spawn_model_path} '
+             '-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'],
+        output='screen'
+    )
+
+    # ============================ 7. robot_state_publisher ===========================
+    robot_description = ParameterValue(
+        Command(['xacro ', LaunchConfiguration('model')]),
+        value_type=str
+    )
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': robot_description,
+            'use_sim_time': True,
+            'publish_frequency': 30.0,
+        }],
+    )
+
+    # ============================ 8. joint_state_publisher ===========================
+    joint_state_publisher_node = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time}],
+    )
+
+    # ============================ 9. slam_toolbox 建图节点 ============================
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[
+            slam_params_file,
+            {
+                'use_sim_time': use_sim_time,
+                'map_file_name': map_file_name,
+            },
+        ],
+    )
+
+    # ============================ 10. 组装 LaunchDescription =========================
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='使用仿真时间（Gazebo=true, 实车=false)'),
+        DeclareLaunchArgument(
+            'slam_params_file',
+            default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'),
+            description='slam_toolbox 参数配置文件路径'),
+        DeclareLaunchArgument(
+            'map_file_name',
+            default_value='',
+            description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
+        DeclareLaunchArgument(
+            'spawn_x', default_value='-4.311092',
+            description='机器人初始 x 坐标（m）'),
+        DeclareLaunchArgument(
+            'spawn_y', default_value='-4.299756',
+            description='机器人初始 y 坐标（m）'),
+        DeclareLaunchArgument(
+            'spawn_z', default_value='0.0',
+            description='机器人初始 z 坐标（m）'),
+        DeclareLaunchArgument(
+            'spawn_yaw', default_value='0.0',
+            description='机器人初始偏航角（rad）'),
+        model,
+
+        set_gazebo_model_path,
+        set_spawn_x,
+        set_spawn_y,
+        set_spawn_z,
+        set_spawn_yaw,
+        start_gazebo_cmd,
+        spawn_entity_cmd,
+        robot_state_publisher_node,
+        joint_state_publisher_node,
+        slam_toolbox_node,
+    ])

src/navigation/gc_navigation2_slamtoolbox/maps/my_map.yaml

@@ -0,0 +1,7 @@
+image: my_map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-2.15, -5.81, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/navigation/gc_navigation2_slamtoolbox/maps/zhihui.yaml

@@ -0,0 +1,7 @@
+image: zhihui.pgm
+mode: trinary
+resolution: 0.05
+origin: [-4.98, -4.71, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/navigation/gc_navigation2_slamtoolbox/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gc_navigation2_slamtoolbox</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="guoch@todo.todo">guoch</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_amcl.yaml

@@ -0,0 +1,413 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_link"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 10.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: True
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 20
+    default_server_timeout: 20
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 30.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.0
+      wz_max: 1.9
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      # Option to replace Cost and use Obstacles instead
+      # ObstaclesCritic:
+      #   enabled: true
+      #   cost_power: 1
+      #   repulsion_weight: 1.5
+      #   critical_weight: 20.0
+      #   consider_footprint: false
+      #   collision_cost: 10000.0
+      #   collision_margin_distance: 0.1
+      #   near_goal_distance: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+      # VelocityDeadbandCritic:
+      #   enabled: true
+      #   cost_power: 1
+      #   cost_weight: 35.0
+      #   deadband_velocities: [0.05, 0.05, 0.05]
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: True
+      transform_tolerance: 0.5
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: True
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false # whether or not to downsample the map
+      downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
+      tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
+      allow_unknown: true # allow traveling in unknown space
+      max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+      max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution
+      max_planning_time: 5.0 # max time in s for planner to plan, smooth
+      motion_model_for_search: "REEDS_SHEPP" # or REEDS_SHEPP  可以倒车
+      angle_quantization_bins: 72 # Number of angle bins for search
+      analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach.
+      analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
+      minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle
+      reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1
+      change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0
+      non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1
+      cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+      retrospective_penalty: 0.015
+      lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters.
+      cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
+      viz_expansions: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.
+      smooth_path: True # If true, does a simple and quick smoothing post-processing to the path
+
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    # drive_on_heading:
+    #   plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.5
+    use_sim_time: True
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: True
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml

@@ -0,0 +1,276 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: False
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+    default_nav_through_poses_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_smooth_path_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_drive_on_heading_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_globally_updated_goal_condition_bt_node
+    - nav2_is_path_valid_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_path_expiring_timer_condition
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+    - nav2_controller_cancel_bt_node
+    - nav2_path_longer_on_approach_bt_node
+    - nav2_wait_cancel_bt_node
+    - nav2_spin_cancel_bt_node
+    - nav2_back_up_cancel_bt_node
+    - nav2_drive_on_heading_cancel_bt_node
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+      desired_linear_vel: 0.3
+      max_angular_vel: 1.9
+      min_angular_vel: 0.2
+      lookahead_dist: 0.5
+      min_lookahead_dist: 0.3
+      max_lookahead_dist: 0.8
+      lookahead_time: 1.5
+      rotate_to_heading_angular_vel: 0.8
+      transform_tolerance: 0.5
+      use_velocity_scaled_lookahead_dist: True
+      min_approach_linear_vel: 0.1
+      approach_velocity_scaling_dist: 0.5
+      use_collision_detection: True
+      max_allowed_time_to_collision: 1.0
+      use_cost_regulated_linear_velocity_scaling: True
+      cost_scaling_dist: 0.6
+      cost_scaling_gain: 1.0
+      regulated_linear_scaling_min_radius: 0.35
+      regulated_linear_scaling_min_speed: 0.1
+      use_rotate_to_heading: False
+      rotate_to_heading_min_angle: 0.5
+      max_robot_pose_search_dist: 10.0
+      use_interpolation: False
+      allow_reversing: True
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 2.0
+      transform_tolerance: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: True
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
+      footprint_padding: 0.02
+      plugins:
+      - voxel_layer
+      - inflation_layer
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.3
+      voxel_layer:
+        plugin: nav2_costmap_2d::VoxelLayer
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: LaserScan
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      transform_tolerance: 2.0
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: True
+      plugins:
+      - static_layer
+      - obstacle_layer
+      - inflation_layer
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: LaserScan
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: nav2_costmap_2d::StaticLayer
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.3
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+planner_server:
+  ros__parameters:
+    planner_plugins:
+    - GridBased
+    use_sim_time: False
+    GridBased:
+      plugin: nav2_smac_planner/SmacPlannerHybrid
+      downsample_costmap: False
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: True
+      max_iterations: 10000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: REEDS_SHEPP
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 2.0
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.5
+      reverse_penalty: 1.3
+      change_penalty: 0.0
+      non_straight_penalty: 0.0
+      cost_penalty: 5.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: False
+      viz_expansions: False
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.4
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: True
+        refinement_num: 4
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins:
+    - simple_smoother
+    simple_smoother:
+      plugin: nav2_smoother::SimpleSmoother
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins:
+    - backup
+    backup:
+      plugin: nav2_behaviors/BackUp
+      backup_dist: 0.8
+      backup_speed: 0.18
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 2.0
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: False
+    waypoint_task_executor_plugin: wait_at_waypoint
+    wait_at_waypoint:
+      plugin: nav2_waypoint_follower::WaitAtWaypoint
+      enabled: True
+      waypoint_pause_duration: 200

src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak

@@ -0,0 +1,343 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+    default_nav_to_pose_bt_xml: /home/sunrise/yiliao_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
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 20
+      model_dt: 0.05
+      batch_size: 200
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.0
+      wz_max: 1.9
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 7.0  # PathAlignCritic
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.2
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      transform_tolerance: 0.5
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: False
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.2
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: true
+      max_iterations: 100000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
+      change_penalty: 0.0
+      non_straight_penalty: 0.5
+      cost_penalty: 5.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"    # 需保留以匹配默认 BT XML
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/navigation/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2

@@ -0,0 +1,343 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+    default_nav_to_pose_bt_xml: /home/sunrise/yiliao_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
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 20
+      model_dt: 0.05
+      batch_size: 200
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.8
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.0
+      wz_max: 1.9
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 2.0
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 8.0
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0  # PathAlignCritic
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 10
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 1.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 2.0
+      transform_tolerance: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.3
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      transform_tolerance: 2.0
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: False
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.3
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: true
+      max_iterations: 100000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 2.0
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
+      change_penalty: 0.0
+      non_straight_penalty: 0.0
+      cost_penalty: 5.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.4
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 4
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"    # 需保留以匹配默认 BT XML
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 2.0
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>zhihui</name>
+    <version>1.0</version>
+    <sdf version="1.7">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/navigation/gc_navigation2_slamtoolbox/world/zhihui/model.sdf

@@ -0,0 +1,471 @@
+<?xml version='1.0'?>
+<sdf version='1.7'>
+  <model name='zhihui'>
+    <pose>-2.11635 -1.73906 0 0 -0 0</pose>
+    <link name='Wall_12'>
+      <collision name='Wall_12_Collision'>
+        <geometry>
+          <box>
+            <size>1.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_12_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-1.61999 -0.744014 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_15'>
+      <collision name='Wall_15_Collision'>
+        <geometry>
+          <box>
+            <size>2.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_15_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>2.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>1.62754 -0.744014 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_17'>
+      <collision name='Wall_17_Collision'>
+        <geometry>
+          <box>
+            <size>1 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_17_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-0.827543 -0.3078 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_19'>
+      <collision name='Wall_19_Collision'>
+        <geometry>
+          <box>
+            <size>1 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_19_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>0.327543 -0.319014 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_2'>
+      <collision name='Wall_2_Collision'>
+        <geometry>
+          <box>
+            <size>5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_2_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>0.062177 -2.89677 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_29'>
+      <collision name='Wall_29_Collision'>
+        <geometry>
+          <box>
+            <size>3.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_29_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-0.305988 0.758719 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_31'>
+      <collision name='Wall_31_Collision'>
+        <geometry>
+          <box>
+            <size>3.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_31_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-0.300519 2.09188 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_33'>
+      <collision name='Wall_33_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_33_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>1.65085 1.41824 0 0 -0 -1.5708</pose>
+    </link>
+    <link name='Wall_36'>
+      <collision name='Wall_36_Collision'>
+        <geometry>
+          <box>
+            <size>2.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_36_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>2.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>2.3125 1.40828 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_38'>
+      <collision name='Wall_38_Collision'>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_38_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-0.19189 2.74413 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_4'>
+      <collision name='Wall_4_Collision'>
+        <geometry>
+          <box>
+            <size>2 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_4_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>2 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>2.6265 -1.8264 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_40'>
+      <collision name='Wall_40_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_40_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-2.25486 1.43754 0 0 -0 -1.5708</pose>
+    </link>
+    <link name='Wall_44'>
+      <collision name='Wall_44_Collision'>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_44_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-2.89932 1.47176 0 0 -0 -1.5708</pose>
+    </link>
+    <link name='Wall_46'>
+      <collision name='Wall_46_Collision'>
+        <geometry>
+          <box>
+            <size>2.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_46_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>2.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-1.87754 0.1172 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_48'>
+      <collision name='Wall_48_Collision'>
+        <geometry>
+          <box>
+            <size>2.13496 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_48_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>2.13496 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>1.32002 0.107134 0 0 -0 0.001157</pose>
+    </link>
+    <link name='Wall_6'>
+      <collision name='Wall_6_Collision'>
+        <geometry>
+          <box>
+            <size>2 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_6_Visual'>
+        <pose>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>2 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+        <meta>
+          <layer>0</layer>
+        </meta>
+      </visual>
+      <pose>-2.50971 -1.81518 0 0 -0 1.5708</pose>
+    </link>
+    <static>1</static>
+  </model>
+</sdf>