TEB Local Planner lagging/frequency drop

edit

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

https://github.com/rst-tu-dortmund/te...

https://github.com/rst-tu-dortmund/te...