# NOTE: When using the teb_local_planner, make sure to set the local costmap
# resolution high (for example 0.1 m), otherwise the optimization will take
# forever (around 10 minutes for each iteration).
base_local_planner: teb_local_planner/TebLocalPlannerROS
TebLocalPlannerROS:
  # Trajectory
  teb_autosize: true                       # Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)
  dt_ref: 0.3                              # Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)
  dt_hysteresis: 0.1                       # Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref
  global_plan_overwrite_orientation: false # Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically
  allow_init_with_backwards_motion: false  # If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)
  max_global_plan_lookahead_dist: 3.0      # Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]
  force_reinit_new_goal_dist: 1.0          # Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
  feasibility_check_no_poses: 5            # Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval
  global_plan_viapoint_sep: -0.1           # Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]
  via_points_ordered: false                # If true, the planner adheres to the order of via-points in the storage container
  exact_arc_length: false                  # If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.
  publish_feedback: false                  # Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)

  # Robot
  max_vel_x: 0.8                           # Maximum translational velocity of the robot
  max_vel_x_backwards: 0.2                 # Maximum translational velocity of the robot for driving backwards
  max_vel_y: 0.0                           # Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
  max_vel_theta: 1.0                       # Maximum angular velocity of the robot
  acc_lim_x: 1.5                           # Maximum translational acceleration of the robot
  acc_lim_y: 0.0                           # Maximum strafing acceleration of the robot
  acc_lim_theta: 2.0                       # Maximum angular acceleration of the robot
  min_turning_radius: 0.0                  # Minimum turning radius of a carlike robot (diff-drive robot: zero)
  wheelbase: 1.0                           # The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
  cmd_angle_instead_rotvel: false          # Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
  is_footprint_dynamic: false              # If true, update the footprint before checking trajectory feasibility
  footprint_model:
    type: "polygon"
    vertices: [[0.506,-0.32],[0.506,0.32],[-0.454,0.32],[-0.454,-0.32]]

  # Goal tolerance
  xy_goal_tolerance: 0.03                  # Allowed final euclidean distance to the goal position
  yaw_goal_tolerance: 0.08                 # Allowed final orientation error to the goal orientation
  free_goal_vel: false                     # Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)

  # Obstacles
  min_obstacle_dist: 0.4                   # Minimum desired separation from obstacles
  inflation_dist: 0.1                      # Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
  dynamic_obstacle_inflation_dist: 0.6     # Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
  include_dynamic_obstacles: false         # Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.
  include_costmap_obstacles: true          # Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)
  legacy_obstacle_association: false       # If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).
  obstacle_association_force_inclusion_factor: 1.5  # The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
  obstacle_association_cutoff_factor: 5.0           # See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
  costmap_obstacles_behind_robot_dist: 1.5 # Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)
  obstacle_poses_affected: 10              # The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well
  #costmap_converter_plugin: ""            #
  #costmap_converter_spin_thread: true     #
  #costmap_converter_rate: 5               #

  # Optimization
  no_inner_iterations: 5                   # Number of solver iterations called in each outerloop iteration
  no_outer_iterations: 4                   # Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations
  optimization_activate: true              # Activate the optimization
  optimization_verbose: false              # Print verbose information
  penalty_epsilon: 0.1                     # Add a small safty margin to penalty functions for hard-constraint approximations
  weight_max_vel_x: 2.0                    # Optimization weight for satisfying the maximum allowed translational velocity
  weight_max_vel_y: 2.0                    # Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)
  weight_max_vel_theta: 1.0                # Optimization weight for satisfying the maximum allowed angular velocity
  weight_acc_lim_x: 1.0                    # Optimization weight for satisfying the maximum allowed translational acceleration
  weight_acc_lim_y: 1.0                    # Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)
  weight_acc_lim_theta: 1.0                # Optimization weight for satisfying the maximum allowed angular acceleration
  weight_kinematics_nh: 1000.0             # Optimization weight for satisfying the non-holonomic kinematics
  weight_kinematics_forward_drive: 1.0     # Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)
  weight_kinematics_turning_radius: 1.0    # Optimization weight for enforcing a minimum turning radius (carlike robots)
  weight_optimaltime: 1.0                  # Optimization weight for contracting the trajectory w.r.t transition time
  weight_obstacle: 50.0                    # Optimization weight for satisfying a minimum seperation from obstacles
  weight_inflation: 0.1                    # Optimization weight for the inflation penalty (should be small)
  weight_dynamic_obstacle: 50.0            # Optimization weight for satisfying a minimum seperation from dynamic obstacles
  weight_dynamic_obstacle_inflation: 0.1   # Optimization weight for the inflation penalty of dynamic obstacles (should be small)
  weight_viapoint: 1.0                     # Optimization weight for minimizing the distance to via-points
  weight_adapt_factor: 2.0                 # Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.

  # Homotopy Class Planner
#  enable_homotopy_class_planning: true    #
  enable_multithreading: true              # Activate multiple threading for planning multiple trajectories in parallel
#  simple_exploration: false               #
  max_number_classes: 2                    # Specify the maximum number of allowed alternative homotopy classes (limits computational effort)
  selection_cost_hysteresis: 1.0           # Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)
  selection_prefer_initial_plan: 0.95      # Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)
  selection_obst_cost_scale: 100.0         # Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)
  selection_viapoint_cost_scale: 1.0       # Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)
  selection_alternative_time_cost: false   # If true, time cost is replaced by the total transition time.
  roadmap_graph_no_samples: 15             # Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off
  roadmap_graph_area_width: 5.0            # Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)
  roadmap_graph_area_length_scale: 1.0     # The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)
  h_signature_prescaler: 1.0               # Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)
  h_signature_threshold: 0.1               # Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold
#  obstacle_keypoint_offset: 0.1           #
  obstacle_heading_threshold: 0.45         # Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)
  viapoints_all_candidates: true           # If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).
  visualize_hc_graph: false                # Visualize the graph that is created for exploring new homotopy classes
  visualize_with_time_as_z_axis_scale: 0.0 # If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.

  # Recovery
  shrink_horizon_backup: true              # Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.
  oscillation_recovery: true               # Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).

  odom_topic: odom
  map_frame: odom
