TEB oscillates when close to goal in tight spaces

edit

I have a differential drive robot that I'm commanding to visit a series of goal points around the perimeter of a stationary object. The setup utilizes move_base with GlobalPlanner and TebLocalPlannerROS. When the goal is in a tight space, the system will occasionally oscillate between short forward and reverse movements as a way to slowly approach the goal. A snapshot of the behavior is below, with the local path that is frequently redrawn in orange and the goal the brown arrow.

In this test, the system got stuck oscillating for about 50 seconds, and my guess is it was able to wiggle itself close enough to the goal to be within the xy_goal_tolerance. I'm uncertain why the system exhibited this behavior when it was able to smoothly navigate up to this point. Included is a bag of the test in question, with the 'stuck' behavior occurring roughly between the 20 and 75 second mark.

bag zip link

Here are my setup parameters:

global/local planner

controller_frequency: 4.6
recovery_behaviour_enabled: true

GlobalPlanner:
  use_dijkstra: true
  allow_unknown: true
  #use_grid_path: false
  default_tolerance: 0.1
  old_navfn_behavior: false
  outline_map: false
  orientation_mode: 0

TebLocalPlannerROS:
  odom_topic: "/odometry/filtered_map"
  map_frame: "/map"

  footprint_model:
    type: "point"
  acc_lim_theta: 0.5
  acc_lim_x: 0.5
  acc_lim_y: 0.5
  allow_init_with_backwards_motion: false
  cmd_angle_instead_rotvel: false
  costmap_obstacles_behind_robot_dist: 1.5
  dt_hysteresis: 0.1
  dt_ref: 0.4
  dynamic_obstacle_inflation_dist: 0.6
  enable_homotopy_class_planning: false
  enable_multithreading: true
  exact_arc_length: false
  feasibility_check_no_poses: 3
  force_reinit_new_goal_angular: 0.5
  force_reinit_new_goal_dist: 0.5
  free_goal_vel: true
  global_plan_overwrite_orientation: true
  global_plan_viapoint_sep: -1.0
  h_signature_prescaler: 1.0
  h_signature_threshold: 0.1
  include_costmap_obstacles: true
  include_dynamic_obstacles: false
  inflation_dist: 0.65
  is_footprint_dynamic: false
  legacy_obstacle_association: false
  max_global_plan_lookahead_dist: 5.0
  max_number_classes: 5
  max_vel_theta: 0.5
  max_vel_x: 0.7
  max_vel_x_backwards: 0.3
  max_vel_y: 0.0
  min_obstacle_dist: 0.57
  min_turning_radius: 0.0
  no_inner_iterations: 4
  no_outer_iterations: 4
  obstacle_association_cutoff_factor: 5.0
  obstacle_association_force_inclusion_factor: 1.5
  obstacle_cost_exponent: 1.0
  obstacle_heading_threshold: 0.45
  obstacle_poses_affected: 30
  optimization_activate: true
  optimization_verbose: false
  oscillation_recovery: true
  penalty_epsilon: 0.15
  publish_feedback: false
  roadmap_graph_area_length_scale: 1.0
  roadmap_graph_area_width: 5.0
  roadmap_graph_no_samples: 15
  selection_alternative_time_cost: false
  selection_cost_hysteresis: 1.0
  selection_obst_cost_scale: 2.0
  selection_prefer_initial_plan: 0.95
  selection_viapoint_cost_scale: 1.0
  shrink_horizon_backup: true
  switching_blocking_period: 0.0
  teb_autosize: true
  via_points_ordered: false
  viapoints_all_candidates: true
  visualize_hc_graph: false
  visualize_with_time_as_z_axis_scale: 0.0
  weight_acc_lim_theta: 1.0
  weight_acc_lim_x: 1.0
  weight_acc_lim_y: 1.0
  weight_adapt_factor: 2.0
  weight_dynamic_obstacle: 50.0
  weight_dynamic_obstacle_inflation: 0.1
  weight_inflation: 0.1
  weight_kinematics_forward_drive:  200.0
  weight_kinematics_nh: 2500.0
  weight_kinematics_turning_radius: 1.0
  weight_max_vel_theta: 1.0
  weight_max_vel_x: 2.0
  weight_max_vel_y: 2.0
  weight_obstacle: 400.0
  weight_optimaltime: 1.0
  weight_shortest_path: 0.0
  weight_viapoint: 20.0
  wheelbase: 1.0
  xy_goal_tolerance: 0.2
  yaw_goal_tolerance: 0.15

costmap_common

footprint: [[-0.5, -0.33], [-0.5, 0.33], [0.5, 0.33], [0.5, -0.33]]
footprint_padding: 0.01 #0.2

robot_base_frame: base_link
update_frequency: 1.0
publish_frequency: 1.0
transform_tolerance: 0.5

resolution: 0.1 #0.05

#layer definitions
static_layer:
    map_topic: /projected_map
    first_map_only: false
    subscribe_to_updates: false

obstacle_layer:
  observation_sources: point_cloud_sensor
  point_cloud_sensor: {data_type: PointCloud2, clearing: false, marking: true, topic: cloud_in, sensor_frame: velodyne , max_obstacle_height: 0.8,   track_unknown_space: true}

inflation_layer:
    enabled: true
    inflation_radius: 0.3

costmap_global

 rolling_window: false
publish_frequency: 10.0
resolution: 0.10
global_frame: map
static_map: true

robot_base_frame: base_link

origin_x: 0 ...