TEB Local Planner lagging/frequency drop
Hi, I have the following problem with teb local planner. Sometimes when a robot is driving the planner stops working for a while, Freuency drops then from 25hz to one loop for 5-50s, yes 50 seconds! Then it never turns back to normal but it oscillates around 5hz.
In the log message, I can only see that control loop missed desired rate, the issue occurs in simulation and real robot, I'm using static global map and small 2,5x2,5 local costmap.
I have enough computer power because it's running at a server.
Is there any useful information I can provide to you?
Issue seems to be independent of teb version (tested on 0.6.14,0.6.13,0.6.0) and from move_base (using deb pkg and build from source the same situation)
ROS Kinetic
Here is my teb config file, but the issue also happens when using default config :
TebLocalPlannerROS:
odom_topic: odom
# Trajectory
teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
allow_init_with_backwards_motion: False
max_global_plan_lookahead_dist: 5.0
feasibility_check_no_poses: 5
exact_arc_length: True
# Robot
max_vel_x: 0.25
max_vel_x_backwards: 0.2
max_vel_theta: 1.5
acc_lim_x: 0.25
acc_lim_theta: 1.0
min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
wheelbase: 0.0
footprint_model:
#type: "point"
type: "two_circles"
front_offset: 0.1 # for type "two_circles"
front_radius: 0.2 # for type "two_circles"
rear_offset: 0.1 # for type "two_circles"
rear_radius: 0.2 # for type "two_circles"
# GoalTolerance
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.05
free_goal_vel: False
# Obstacles
min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point".
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 0.5
obstacle_poses_affected: 5
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 2
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
weight_max_vel_x: 10
weight_max_vel_y: 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
weight_adapt_factor: 2
# Homotopy Class Planner
enable_homotopy_class_planning: True
enable_multithreading: True
simple_exploration: False
max_number_classes: 2
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False
selection_preger_initial_plan: 0.95
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
EDIT: This is similar to
Link to video showing how this affect navigation . At 25s you can see that teb stopped working.
https://www.youtube.com/watch?v=-T9Zq...