planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    planner_plugins: ["GridBased"]
    
    GridBased:
      # Use SmacPlannerLattice plugin
      plugin: "nav2_smac_planner::SmacPlannerLattice"
      
      # --- Core Planning Controls ---
      allow_unknown: true                       # Allow planning through unknown space
      tolerance: 0.25                           # Goal tolerance for planning (meters)
      max_iterations: 1000000                   # Maximum planning iterations
      max_on_approach_iterations: 1000          # Max iterations when approaching goal
      max_planning_time: 2.0                    # Maximum time for planning (seconds)
      
      # --- Heuristic Penalties (tune as-needed) ---
      # These control the cost of different motion types
      analytic_expansion_ratio: 3.5              # (**USED 200** in the release version)  Default — enables analytic expansion with omni SE2 paths
      analytic_expansion_max_length: 10.0       # (**USED 0.1** in the release version) Moderate length to limit abrupt heading transitions
      analytic_expansion_max_cost: 200.0        # Max cost for analytic expansion
      analytic_expansion_max_cost_override: false  # Override max cost check

      reverse_penalty: 1.0                      # No penalty — holonomic can reverse freely
      change_penalty: 0.05                      # Penalty for changing direction
      non_straight_penalty: 1.0                 # No penalty — lateral strafing is native for holonomic
      cost_penalty: 2.0                         # Penalty multiplier for costmap cost
      rotation_penalty: 1.0                     # No penalty — holonomic can pivot freely
      retrospective_penalty: 0.015              # Penalty for retrospective search
      
      # --- Lattice-Specific Parameters ---
      # Path to the motion primitives file (MUST be absolute path at runtime)
      # This will be set via launch file to ensure proper path resolution
      lattice_filepath: ""
      
      lookup_table_size: 20.0                   # Size of lookup table for heuristic
      cache_obstacle_heuristic: true            # Cache obstacle heuristic (big speedup if map is mostly static)
      allow_reverse_expansion: true             # Allow reverse motion primitives
      
      # --- Path Smoothing ---
      smooth_path: true                         # Enable path smoothing
      smoother:
        max_iterations: 800                     # Max smoothing iterations
        w_smooth: 0.3                           # Weight for smoothness
        w_data: 0.2                             # Weight for data fidelity
        tolerance: 1.0e-10                      # Convergence tolerance
        do_refinement: true                     # Enable refinement
        refinement_num: 2                       # Number of refinement passes
      
      # --- Goal Heading Search ---
      # Controls how the planner handles goal heading
      # "DEFAULT" = use specified goal heading
      # "ALL_DIRECTION" = search for best goal heading from all directions
      goal_heading_mode: "DEFAULT"
      coarse_search_resolution: 1               # Resolution for coarse search (if using ALL_DIRECTION)