controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.01
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.01
    progress_checker_plugins: ["progress_checker"]  # progress_checker_plugin: "progress_checker" For Humble and older
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.1
      yaw_goal_tolerance: 0.05
      stateful: True
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.25
      lookahead_dist: 5.0
      min_lookahead_dist: 0.1
      max_lookahead_dist: 5.0
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 1.8
      transform_tolerance: 0.5
      use_velocity_scaled_lookahead_dist: true
      min_approach_linear_velocity: 0.1
      approach_velocity_scaling_dist: 0.6
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 2.0
      use_regulated_linear_velocity_scaling: true
      use_fixed_curvature_lookahead: false
      curvature_lookahead_dist: 1.5
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 5.0
      regulated_linear_scaling_min_speed: 0.15
      use_rotate_to_heading: false
      allow_reversing: true
      rotate_to_heading_min_angle: 0.785
      max_angular_accel: 3.2
      max_robot_pose_search_dist: 10.0

  ros__parameters:
    expected_planner_frequency: 2.0
    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"        # Hybrid-A* Dubin, Redds-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: 16.0            # If the length is too far, reject this expansion. This prevents shortcutting of search with its penalty functions far out from the goal itself (e.g. so we don’t reverse half-way across open maps or cut through high cost zones). This should never be smaller than 4-5x the minimum turning radius being used, or planning times will begin to spike.
      analytic_expansion_max_cost: 200.0            # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
      analytic_expansion_max_cost_override: False   #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      minimum_turning_radius: 4.7                   # minimum turning radius in m of path / vehicle
      reverse_penalty: 1.1                          # 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: 8.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                  # Heuristic penalty to apply to SE2 node penalty. Causes Hybrid-A* to prefer later maneuvers before earlier ones along the path. Saves search time since earlier (shorter) branches are not expanded until it is necessary. Must be >= 0.0 and <= 1.0. Must be 0.0 to be fully admissible.
      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.
      debug_visualizations: False                   # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
      use_quadratic_cost_penalty: False
      downsample_obstacle_heuristic: True
      allow_primitive_interpolation: False
      smooth_path: False                             # 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