# ============================================================================
# planner.yaml — Nav2 minimal global planning stack
# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
# ============================================================================

# ---------------------------------------------------------------------------
# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
# 来源: slam_toolbox_mapping.yaml
# ---------------------------------------------------------------------------
slam_toolbox:
  ros__parameters:
    use_sim_time: False

    # 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 base
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan
    mode: mapping
    use_map_saver: true

    # Debug & performance
    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02
    map_update_interval: 3.0
    resolution: 0.05
    min_laser_range: 0.15
    max_laser_range: 20.0
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    stack_size_to_use: 40000000
    enable_interactive_mode: true

    # Mapping
    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

    # Loop closure
    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

    # Scan matching
    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

    # Matcher params
    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
    scan_queue_size: 20

# ---------------------------------------------------------------------------
# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
# 来源: gc_navigation_slam.yaml planner_server section
# ---------------------------------------------------------------------------
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.35
      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

# ---------------------------------------------------------------------------
# global_costmap — 全局代价地图（订阅 /scan + /map）
# 来源: gc_navigation_slam.yaml global_costmap section
# ---------------------------------------------------------------------------
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: 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

# ---------------------------------------------------------------------------
# local_costmap — 局部代价地图（obstacle_detector 可注入）
# 来源: gc_navigation_slam.yaml local_costmap section
# ---------------------------------------------------------------------------
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.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
