TEB local planner follow path, toggles between targets

edit

Hello community,

my robot should follow a given path (floor coverage). Also it should take obstacles into account, which leads me to use TEB_local_planner for this task. I know that it is not ideal for straight path following, but it's great for obstacle avoidance.

However when using TEB, my robot oscillates between two points (see video). It seems to me, that TEB changes/toggles between two points of my path.

What causes this and do someone know, how to overcome it?

https://youtu.be/SgjgB2sc56E

this is how I send the path to MBF

    exePathGoal.path = path.path;
    exePathGoal.angle_tolerance = 5.0 * (M_PI / 180.0);
    exePathGoal.dist_tolerance = 0.2;
    exePathGoal.tolerance_from_action = true;
    exePathGoal.controller = "FTCPlanner";
    mbfClientExePath->sendGoal(exePathGoal);

When doing the same with a simple FTCPlanner, it works and robot follows the path, but doesn't care about obstacles as this hasn't been implemented in the FTCPlanner I use.

This is TEB configuration

TebLocalPlannerROS:

 odom_topic: odom
 map_frame: map

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: False #True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5
 allow_init_with_backwards_motion: True # allow backward movement at start to reach the goal (i.E. if behind robot)

 # Robot

 max_vel_x: 0.3
 max_vel_x_backwards: 0.2
 max_vel_theta: 0.3
 acc_lim_x: 0.5
 acc_lim_theta: 0.5
 min_turning_radius: 0.0 # rotate in place
 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.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"   
   vertices: [ [0.45, 0.2], [-0.15,  0.2], [-0.15, -0.2], [ 0.45, -0.2] ] # for type "polygon"
 # GoalTolerance

 xy_goal_tolerance: 0.1
 yaw_goal_tolerance: 0.1
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.1
 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: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 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: 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