ROS2无人车学习第三章

# slam_toolbox 建图模式参数（仿真专用）
# 基于 /opt/ros/humble/share/slam_toolbox/config/mapper_params_online_async.yaml 复制，
# 仅修改 base_frame（本工作区机器人 TF 树以 base_link 为基座，无 base_footprint）。
# 用于 mapping_sim.sh：Gazebo + slam_toolbox(online_async) 建图。
slam_toolbox:
  ros__parameters:

    # ---- 后端图优化求解器（Ceres Solver） ----
    solver_plugin: solver_plugins::CeresSolver                           # 求解器类型
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY                           # 稀疏 Cholesky 分解
    ceres_preconditioner: SCHUR_JACOBI                                    # Schur 补 + Jacobi 预处理
    ceres_trust_strategy: LEVENBERG_MARQUARDT                             # LM 信赖域策略
    ceres_dogleg_type: TRADITIONAL_DOGLEG                                 # Dogleg 方法（仅 DOGLEG 策略时生效）
    ceres_loss_function: None                                             # 鲁棒核函数（None=无，可选 HuberLoss）

    # ---- 坐标系与话题 ----
    odom_frame: odom                                                      # 里程计坐标系
    map_frame: map                                                         # 全局地图坐标系
    base_frame: base_link                                                  # 机器人基座坐标系（本车为 base_link）
    scan_topic: /scan                                                      # 激光雷达话题名
    use_map_saver: true                                                    # 是否允许运行时保存地图
    mode: mapping                                                          # 工作模式：mapping / localization

    # ---- 通用参数 ----
    debug_logging: false                                                   # 启用调试日志
    throttle_scans: 1                                                      # 扫描降采样（1=全处理）
    transform_publish_period: 0.02                                         # map->odom TF 发布间隔（秒）
    map_update_interval: 5.0                                               # 地图栅格更新间隔（秒）
    resolution: 0.05                                                       # 地图分辨率（米/像素）
    min_laser_range: 0.15                                                  # 激光有效最小距离（米）
    max_laser_range: 20.0                                                  # 激光有效最大距离（米）
    minimum_time_interval: 0.5                                             # 帧间最小时间间隔（秒）
    transform_timeout: 0.2                                                 # TF 查询超时（秒）
    tf_buffer_duration: 30.                                                # TF 缓冲区时长（秒）
    stack_size_to_use: 40000000                                            # 线程栈大小（字节，防深递归崩溃）
    enable_interactive_mode: true                                          # 允许 RViz 交互面板控制

    # ---- 前端扫描匹配 ----
    use_scan_matching: true                                                # 启用激光扫描匹配
    use_scan_barycenter: true                                              # 使用扫描质心辅助对齐
    minimum_travel_distance: 0.3                                           # 新增节点最小平移距离（米）
    minimum_travel_heading: 0.3                                            # 新增节点最小旋转角度（弧度）
    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                                 # 精细匹配最小响应阈值

    # ---- 前端相关性搜索空间（scan-to-map） ----
    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                                      # 精细搜索角度偏移（弧度，≈0.2°）
    coarse_search_angle_offset: 0.349                                      # 粗搜索角度偏移（弧度，≈20°）
    coarse_angle_resolution: 0.0349                                        # 粗搜索角度步长（弧度，≈2°）
    minimum_angle_penalty: 0.9                                             # 角度匹配最小惩罚因子
    minimum_distance_penalty: 0.5                                          # 距离匹配最小惩罚因子
    use_response_expansion: true                                           # 动态扩展搜索范围
    min_pass_through: 2                                                    # 栅格通过计数阈值
    occupancy_threshold: 0.1                                               # 栅格占据概率阈值

# =============================================================================
# slam_toolbox 定位模式参数配置（仿真专用）
# =============================================================================
# 基于 ROS 2 Humble 官方默认配置修改：
#   /opt/ros/humble/share/slam_toolbox/config/mapper_params_localization.yaml
#
# 修改内容：
#   仅将 base_frame 从默认的 base_footprint 改为 base_link，
#   因为本工作区机器人的 TF 树以 base_link 为基座，没有 base_footprint 坐标系。
#
# 用于 nav2_sim.sh 启动流程：
#   加载 mapping_sim.sh 阶段保存的地图（serialize_map），只做重定位（localization），
#   不再建图更新。
# =============================================================================

slam_toolbox:
  ros__parameters:

    # ---- 后端图优化求解器（Ceres Solver） ----
    solver_plugin: solver_plugins::CeresSolver              # 求解器类型
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY              # 稀疏 Cholesky 分解
    ceres_preconditioner: SCHUR_JACOBI                       # Schur 补 + Jacobi 预处理
    ceres_trust_strategy: LEVENBERG_MARQUARDT                # LM 信赖域策略
    ceres_dogleg_type: TRADITIONAL_DOGLEG                    # Dogleg 方法（仅 DOGLEG 策略时生效）
    ceres_loss_function: None                                # 鲁棒核函数（None=无，可选 HuberLoss）

    # ---- 坐标系与话题 ----
    odom_frame: odom                                         # 里程计坐标系
    map_frame: map                                           # 全局地图坐标系
    base_frame: base_link                                    # 机器人基座坐标系（本车为 base_link）
    scan_topic: /scan                                        # 激光雷达话题名
    mode: localization                                       # 工作模式：localization（仅定位，不更新地图）

    # ---- 通用参数 ----
    debug_logging: false                                     # 启用调试日志
    throttle_scans: 1                                        # 扫描降采样（1=全处理）
    transform_publish_period: 0.02                           # map->odom TF 发布间隔（秒）
    map_update_interval: 5.0                                 # 地图栅格更新间隔（秒）
    resolution: 0.05                                         # 地图分辨率（米/像素）
    min_laser_range: 0.15                                    # 激光有效最小距离（米）
    max_laser_range: 20.0                                    # 激光有效最大距离（米）
    minimum_time_interval: 0.5                               # 帧间最小时间间隔（秒）
    transform_timeout: 0.2                                   # TF 查询超时（秒）
    tf_buffer_duration: 30.                                  # TF 缓冲区时长（秒）
    stack_size_to_use: 40000000                              # 线程栈大小（字节，防深递归崩溃）

    # ---- 前端扫描匹配 ----
    use_scan_matching: true                                  # 启用激光扫描匹配
    use_scan_barycenter: true                                # 使用扫描质心辅助对齐
    minimum_travel_distance: 0.3                             # 新增节点最小平移距离（米）
    minimum_travel_heading: 0.3                              # 新增节点最小旋转角度（弧度）
    scan_buffer_size: 3                                      # 子图保留扫描帧数（定位模式较小，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                   # 精细匹配最小响应阈值

    # ---- 前端相关性搜索空间（scan-to-map） ----
    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                  # 回环搜索模糊标准差
    loop_search_maximum_distance: 3.0                        # 回环搜索半径（米）

    # ---- 位姿方差惩罚 ----
    distance_variance_penalty: 0.5                           # 平移变化惩罚权重
    angle_variance_penalty: 1.0                              # 旋转变化惩罚权重

    # ---- 角度搜索参数 ----
    fine_search_angle_offset: 0.00349                        # 精细搜索角度偏移（弧度，≈0.2°）
    coarse_search_angle_offset: 0.349                        # 粗搜索角度偏移（弧度，≈20°）
    coarse_angle_resolution: 0.0349                          # 粗搜索角度步长（弧度，≈2°）
    minimum_angle_penalty: 0.9                               # 角度匹配最小惩罚因子
    minimum_distance_penalty: 0.5                            # 距离匹配最小惩罚因子
    use_response_expansion: true                             # 动态扩展搜索范围
    min_pass_through: 2                                      # 栅格通过计数阈值
    occupancy_threshold: 0.1                                 # 栅格占据概率阈值