TEB local planner seem to ignore footprint?

[wrmichaelmak]

You can see clearly that the poses generated by TEB local planner does not seem to care about the footprint of my robot.

My parameters are the following:

TebLocalPlannerROS:
 odom_topic: /odom
 map_frame: /map
 # Trajectory
  
 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 max_samples: 500
 global_plan_overwrite_orientation: false
 allow_init_with_backwards_motion: true
 max_global_plan_lookahead_dist: 3.0
 global_plan_viapoint_sep: 0.1
 global_plan_prune_distance: 1
 exact_arc_length: False
 feasibility_check_no_poses: 7
 publish_feedback: False
    
 # Robot
         
 max_vel_x: 0.2
 max_vel_x_backwards: 0.2
 max_vel_y: 0.2
 max_vel_theta: 1
 acc_lim_x: 0.4
 acc_lim_y: 0.4
 acc_lim_theta: 0.5
 min_turning_radius: 0.0 # omni-drive robot (can turn on place!)

 footprint_model:
    type: "polygon"
    vertices: [[0.6, 0.5], [0.6, -0.5], [-0.6, -0.5], [-0.6, 0.5]]

 # GoalTolerance
    
 xy_goal_tolerance: 0.1
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
 complete_global_plan: True
    
 # Obstacles
    
 min_obstacle_dist: 0.7 # This value must also include our robot radius.
 inflation_dist: 0.25
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 5.0
 obstacle_poses_affected: 20
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization
    
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.2
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2
 weight_max_vel_y: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_y: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 20 # WE HAVE A HOLONOMIC ROBOT, JUST ADD A SMALL PENALTY
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1 # must be > 0
 weight_shortest_path: 0
 weight_obstacle: 200
 weight_inflation: 0.2
 weight_dynamic_obstacle: 10
 weight_dynamic_obstacle_inflation: 0.2
 weight_viapoint: 10
 weight_adapt_factor: 2

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.9
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False
 
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False

 # Recovery
 
 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

recovery_behaviors:
  - {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}
  - {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}