Robotics StackExchange | Archived questions

TEB oscillates when close to goal in tight spaces

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.

image description

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.0
origin_y: 0.0

width: 40
height: 40

plugins:
  - {name: static_layer,      type: "costmap_2d::StaticLayer"}
  - {name: inflation_layer,   type: "costmap_2d::InflationLayer"}

costmap_local

 global_frame: map
rolling_window: true
width: 5.5
height: 5.5
resolution: 0.1
transform_tolerance: 0.5

sonar_layer:
  sonar_topic: /sonar_triggered_warning


plugins:
  - {name: static_layer,      type: "costmap_2d::StaticLayer"}
  - {name: sonar_layer,       type: "sonar_layer::SonarLayer"}

For this setup, I'm using a pair of EKF from robot_localization. This test was done in a GPS-denied environment, so the odom and map frames are identical. You'll likely notice there's some pretty bad localization smear while the system is wiggling. I'll leave this for another question for another day, but for completeness here's my localization setup.

odom_frame: odom
base_link_frame: base_link
map_frame: map
world_frame: odom
sensor_timeout: 0.1

transform_timeout: 0.02

two_d_mode: true

frequency: 30
publish_tf: true

print_diagnostics: false

odom0: radlad_velocity_controller/odom
odom0_config: [false, false, false,
               false, false, false,
               true, true, false,
               false, false, false,
               false, false, false]
odom0_differential: false
#odom0_nodelay: true
odom0_queue_size: 10

imu0: imu/data
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              false, true, false,
              false, false, false]
imu0_differential: false
imu0_queue_size: 10
#imu0_nodelay: true
imu0_relative: false
imu0_remove_gravitational_acceleration: false

use_control: false
control_timeout: 0.1
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.2, 0.0, 0.0, 0.0, 0.0, 2.0]


process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

initial_estimate_covariance: [0.01,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0.01,  0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0.01, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0.01,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0.01,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0.01,  0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0.01,  0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0.01,  0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01,   0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0.01,   0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0.01,   0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01,  0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01,  0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.01]

Of course I would appreciate any tips regarding the localization smear issue, but my focus here is the movement behavior. Thank you in advance!

I am using Noetic within Docker on a RPi4 running Raspbian.

Asked by chives_onion on 2021-06-22 17:06:58 UTC

Comments

I'm having difficulty attaching my bag file. I've tried both .bag and .zip, but it doesn't seem to be added. I'm also experiencing major delays when accessing this site right now. Any suggestion of how to provide my bag?

Asked by chives_onion on 2021-06-22 17:13:41 UTC

How big is it? Can you put it into some kind file hosting service and link to it?

Asked by jayess on 2021-06-22 18:15:47 UTC

Thanks @jayess, I've updated the question to include a dropbox link to the bag.

Asked by chives_onion on 2021-06-23 08:08:55 UTC

Answers