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.
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.
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