base_local_planner: nav_core_adapter::LocalPlannerAdapter
LocalPlannerAdapter:
  planner_name: dwb_local_planner::DWBLocalPlanner
DWBLocalPlanner:
  # Robot configuration
  max_vel_x:  1.0
  min_vel_x:  0.1

  max_vel_y: 0.0  # diff drive robot
  min_vel_y: 0.0  # diff drive robot

  max_speed_xy: 1.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
  trans_stopped_velocity: 0.25  #default : 0.25
  max_vel_theta: 1.0    # max_rot_vel: 1.0  # choose slightly less than the base's capability
  min_speed_theta: 0.1  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity

  acc_lim_x: 1.0
  acc_lim_y: 0.0      # diff drive robot
  acc_lim_theta: 1.0
  decel_lim_x: -1.0
  decel_lim_y: -0.0
  decel_lim_theta: -1.0

  # Goal tolerance
  yaw_goal_tolerance: 0.05  # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
  xy_goal_tolerance: 0.10   # xy_goal_tolerance  > (sim_time * min_vel_x)   / 2
  #latch_xy_goal_tolerance: true

  # Whether to split the path into segments or not
  # Requires <https://github.com/locusrobotics/robot_navigation/pull/50>
  split_path: true

  trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator # or dwb_plugins::LimitedAccelGenerator
  sim_time: 1.2
  vx_samples: 15
  vy_samples: 1       # diff drive robot, there is only one sample
  vtheta_samples: 15
  discretize_by_time: false
  angular_granularity: 0.35
  linear_granularity: 0.05
  # angular_granularity: 0.15
  # linear_granularity: 0.05
  # sim_period
  include_last_point: True

  # Goal checking
  goal_checker_name: dwb_plugins::StoppedGoalChecker
  #stateful: true

  # Critics (trajectory scoring)
  critics: [RotateToGoal, Oscillation, ObstacleFootprint, GoalAlign, PathAlign, PathDist, GoalDist]
  RotateToGoal:
    scale: 100.0
    lookahead_time: -1.0
    slowing_factor: 5.0
  ObstacleFootprint:
    scale: 0.01             # default: 0.01  mir: 0.01  - weighting for how much the controller should avoid obstacles
    max_scaling_factor: 0.2 # default: 0.2   mir: 0.2   - how much to scale the robot's footprint when at speed.
    scaling_speed: 0.3     # default: 0.25  mir: 0.25  - absolute velocity at which to start scaling the robot's footprint
    sum_scores: false       # if true, return sum of scores of all trajectory points instead of only last one
  PathAlign:
    scale: 16.0
    forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
  PathDist:
    # aggregation_type: last
    scale: 16.0
  GoalDist:
    scale: 16.0
  GoalAlign:
    # aggregation_type: last
    scale: 16.0
  PathDistPruned:
    scale: 32.0            # default: 32.0  mir: 32.0   - weighting for how much it should stick to the global path plan
    class: 'dwb_critics::PathDistPruned'
  PathProgress:
    scale: 24.0            # default: 24.0  mir: 48.0   - weighting for how much it should attempt to reach its goal
    heading_scale: 0.1
    class: 'dwb_critics::PathProgress'
    xy_local_goal_tolerance: 0.10
    angle_threshold: 0.05  # 45 degrees
  Oscillation:
    # scale: 1.0
    # x_only_threshold: 0.02
    oscillation_reset_dist : 0.05
    oscillation_reset_angle : 0.2
    oscillation_reset_time : -1.0

  # Prune already passed poses from plan
  prune_plan: true
  prune_distance: 1.0   # Old poses farther away than prune_distance (in m) will be pruned.
                        # If the robot ever gets away further than this distance from the plan,
                        # the error "Resulting plan has 0 poses in it" will be thrown and
                        # replanning will be triggered.

  # Debugging
  publish_cost_grid_pc: true
  debug_trajectory_details: true
  publish_evaluation: true
  publish_global_plan: true
  publish_input_params: true
  publish_local_plan: true
  publish_trajectories: true
  publish_transformed_plan: true
  marker_lifetime: 0.5