Newer
Older
orange2022 / src / tsukuba2022 / params / teb_local_planner_params.yaml
@koki koki on 20 Sep 2022 2 KB update
base_local_planner: teb_local_planner/TebLocalPlannerROS

TebLocalPlannerROS:
  
  odom_topic: odom
  map_frame: /map
  
  # Trajectory
  teb_autosize: True
  dt_ref: 0.5 #0.3
  dt_hysteresis: 0.1
  global_plan_overwrite_orientation: True
  max_global_plan_lookahead_dist: 2.0
  feasibility_check_no_poses: 5
  
  # Robot
  max_vel_x: 1.6
  max_vel_x_backwards: 0.2
  max_vel_theta: 0.4
  acc_lim_x: 2.0
  acc_lim_theta: 1.0
  min_turning_radius: 0.0
  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
    type: "polygon"
    # radius: 0.2 # for type "circular"
    # line_start: [-0.3, 0.0] # for type "line"
    # line_end: [0.3, 0.0] # for type "line"
    # front_offset: 0.2 # for type "two_circles"
    # front_radius: 0.2 # for type "two_circles"
    # rear_offset: 0.2 # for type "two_circles"
    # rear_radius: 0.2 # for type "two_circles"
    vertices: [ [0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4] ] # for type "polygon"
  
  # GoalTolerance
  xy_goal_tolerance: 0.3
  yaw_goal_tolerance: 0.3
  free_goal_vel: False
  
  # Obstacles
  min_obstacle_dist: 0.4
  inflation_dist: 0.0 #0.6
  include_costmap_obstacles: True
  costmap_obstacles_behind_robot_dist: 1.0
  obstacle_poses_affected: 30
  costmap_converter_plugin: ""
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
  
  # Optimization
  no_inner_iterations: 4 #5
  no_outer_iterations: 3 #4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.1
  weight_max_vel_x: 1000 #2
  weight_max_vel_theta: 1000 #1
  weight_acc_lim_x: 100 #1
  weight_acc_lim_theta: 100 #1
  weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 100 #1
  weight_kinematics_turning_radius: 1
  weight_optimaltime: 1
  weight_obstacle: 50
  weight_dynamic_obstacle: 10 # not in use yet
  selection_alternative_time_cost: False # not in use yet
  
  # Homotopy Class Planner
  enable_homotopy_class_planning: True
  enable_multithreading: True
  simple_exploration: False
  max_number_classes: 3 #4
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_keypoint_offset: 0.1
  obstacle_heading_threshold: 0.45
  visualize_hc_graph: False