yaw_goal_tolerance: 0.03  
xy_goal_tolerance: 0.02 
min_approach_linear_velocity: 0.05 

LocalPlannerAdapter:
  library_path: liblocal_planner_adapter
PNKXLocalPlanner:
  # Algorithm
  library_path: libpnkx_local_planner
  trajectory_generator_name: LimitedAccelGenerator 
  algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
  algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
  # Goal checking
  goal_checker_name: GoalChecker 

PNKXDockingLocalPlanner: 
  # Algorithm
  library_path: libpnkx_local_planner
  trajectory_generator_name: LimitedAccelGenerator 
  algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
  algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
  # Goal checking
  goal_checker_name: GoalChecker 

PNKXGoStraightLocalPlanner:
  # Algorithm
  library_path: libpnkx_local_planner
  trajectory_generator_name: LimitedAccelGenerator 
  algorithm_nav_name: MKTAlgorithmDiffGoStraight
  # Goal checking
  goal_checker_name: GoalChecker 

PNKXRotateLocalPlanner:
  # Algorithm
  library_path: libpnkx_local_planner
  algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
  trajectory_generator_name: LimitedAccelGenerator 
  # Goal checking
  goal_checker_name: SimpleGoalChecker
  stateful: true
    
LimitedAccelGenerator:
  library_path: libmkt_plugins_standard_traj_generator
  max_vel_x: 1.2
  min_vel_x: -1.2

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

  max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity

  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
  min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity

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

  # Whether to split the path into segments or not
  split_path: true
  sim_time: 1.5
  vx_samples: 15
  vy_samples: 1       
  vtheta_samples: 10
  discretize_by_time: true
  angular_granularity: 0.05
  linear_granularity: 0.1
  # sim_period
  include_last_point: true

MKTAlgorithmDiffPredictiveTrajectory:
  library_path: libmkt_algorithm_diff
  xy_local_goal_tolerance: 0.02
  angle_threshold: 0.47
  index_samples: 60
  follow_step_path: true

  # Kalman filter tuning (filters v and w commands)
  kf_q_v: 0.25
  kf_q_w: 0.8
  kf_r_v: 0.05
  kf_r_w: 0.08
  kf_p0: 0.5
  kf_filter_angular: false

  # Lookahead
  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
  # only when false:
  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
  # only when true:
  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
  max_journey_squared: 0.5                    # Maximum squared journey to consider for goal (default: 0.2) 
  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)

  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
  # only when true:
  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
  angular_decel_zone: 0.1             

  # stoped
  rot_stopped_velocity: 0.05
  trans_stopped_velocity: 0.06
  
  use_final_heading_alignment: true
  final_heading_xy_tolerance: 0.1
  final_heading_angle_tolerance: 0.05
  final_heading_min_velocity: 0.05
  final_heading_kp_angular: 1.2
  final_heading_ki_angular: 0.002
  final_heading_kd_angular: 0.12

MKTAlgorithmDiffGoStraight:
  library_path: libmkt_algorithm_diff
  xy_local_goal_tolerance: 0.02
  angle_threshold: 0.8
  index_samples: 60
  follow_step_path: true

  # Lookahead
  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
  # only when false:
  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
  # only when true:
  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
  lookahead_time: 2.0                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
  min_journey_squared: 1.0                    # Minimum squared journey to consider for goal (default: 0.2) 
  max_journey_squared: 1.0                    # Maximum squared journey to consider for goal (default: 0.2) 
  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)

  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
  # only when true:
  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
  angular_decel_zone: 0.1             

  # stoped
  rot_stopped_velocity: 0.05
  trans_stopped_velocity: 0.06
  
  use_final_heading_alignment: true
  final_heading_xy_tolerance: 0.1
  final_heading_angle_tolerance: 0.05
  final_heading_min_velocity: 0.05
  final_heading_kp_angular: 1.5
  final_heading_ki_angular: 0.2
  final_heading_kd_angular: 0.05

MKTAlgorithmDiffRotateToGoal:
  library_path: libmkt_algorithm_diff
  xy_local_goal_tolerance: 0.02
  angle_threshold: 0.47
  index_samples: 60
  follow_step_path: true

  # Lookahead
  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
  # only when false:
  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
  # only when true:
  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)

  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
  # only when true:
  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
  angular_decel_zone: 0.1             

  # stoped
  rot_stopped_velocity: 0.05
  trans_stopped_velocity: 0.06
  
  use_final_heading_alignment: true
  final_heading_xy_tolerance: 0.1
  final_heading_angle_tolerance: 0.05
  final_heading_min_velocity: 0.05
  final_heading_kp_angular: 2.0
  near_goal_heading_kp: 1.5
  near_goal_heading_ki: 0.0
  near_goal_heading_kd: 0.0
  
GoalChecker:
  library_path: libmkt_plugins_goal_checker

SimpleGoalChecker:
  library_path: libmkt_plugins_goal_checker