looping around dynamic obstacles (when enable_homotopy_class_planning is false)

[67bug]

We are testing out the teb local planner and i have to admit that it works quite well. So, first, thank you.

ROS Version: ros noetic
Vehicle: Differential Drive
Platform: Jetson Xavier NX

The challenge we are having is that if we turn on homtopy class planning, while the planner works quite well, we end up missing a lot of control loops and map update loops. Obviously, this is tied to the computational limitations of the Jetson Xavier NX and everything else we have running on it. We tried multiple things ranging from:

1. reducing the cost map resolution
2. reducing costmap size
3. reducing obstacle_poses_affected
4. reducing max_global_plan_lookahead_dist
5. reducing no_inner_iterations
6. reducing no_outer_iterations

None of these has any significant impact on the number of times we miss the control and map update loops, except for setting enable_homotopy_class_planning: false

This is not desirable as alternate paths are not computed.

Here is one sample screenshot from the rosbag where it is clear that we needed the alternate path computation enabled by homotopy_class_planning

Is there some way to force the computation of a new plan instead of having the kind of "memory" that the planner seems to have of the old plan?

Here are the teb planner settings used

TebLocalPlannerROS:
  # Robot configuration parameters
  acc_lim_x: 0.05
  acc_lim_theta: 0.05 #0.5

  max_vel_x: 1
  max_vel_x_backwards: 0.3 # if backward motion need
  max_vel_theta: 0.418879 

  min_turning_radius: 0 # 0 for diff drive, , used only for Ackermann 

  wheelbase: 0 # 0 for diff drive, used only for Ackermann 
  cmd_angle_instead_rotvel: false

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.5
  xy_goal_tolerance: 0.8 #0.2
  free_goal_vel: true # Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed
  complete_global_plan: false

# Obstacle - Velocity ratio parameters  Line 107 in edge_velocity_obstacle_ratio.h
# if distance to obstacle is less than obstacle_proximity_lower_bound, the linear and angular velocities are reduced to zero, else it is 
  obstacle_proximity_ratio_max_vel: 1 # default 1 Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles
  obstacle_proximity_lower_bound:  0.5 # default 0 Distance to a static obstacle for which the velocity should be lower
  obstacle_proximity_upper_bound: 1.5 # default 0.5 Distance to a static obstacle for which the velocity should be higher

  # Trajectory Configuration Parameters
  teb_autosize: true
  dt_ref: 0.35 #0.35 #fix
  dt_hysteresis: 0.05
  min_samples: 3 #5
  global_plan_overwrite_orientation: false
  max_global_plan_lookahead_dist: 3 #3.0 #3.0 #20.0 Keeping local costmap size into consideration
  force_reinit_new_goal_dist: 1.0
  feasibility_check_no_poses: 3 #20 #4
  publish_feedback: true
  allow_init_with_backwards_motion: false
  exact_arc_length: false
  global_plan_viapoint_sep: 5 #2.0 #0.5 #1.0   set to negative to disable
  via_points_ordered: false
  global_plan_prune_distance: 1

  # Obstacle Parameters
  min_obstacle_dist: 0.55 # Set the inflation radius greater than the min_obstacle_dist and also make sure that the robot follows the global plan more accurately to reduce the oscillations.
  include_costmap_obstacles: true # needed if we are not using the costmap convertor plugin
  costmap_obstacles_behind_robot_dist: 1 #  Since the local costmap is centered at the current robot position, not all obstacles behind the robot must be taken into account. To allow safe turning behaviors, this value should be non-zero. A higher value includes more obstacles for optimization.
  obstacle_poses_affected: 10
  inflation_dist: 0.6
  include_dynamic_obstacles: true
  dynamic_obstacle_inflation_dist: 3
  legacy_obstacle_association: false
  obstacle_association_force_inclusion_factor: 2 # The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
  obstacle_association_cutoff_factor: 5 # See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
  # costmap convertor takes a lot of CPU, turning off for now
  #costmap_converter::CostmapToLinesDBSRANSAC, CostmapToLinesDBSMCCH
  costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" #"costmap_converter::CostmapToPolygonsDBSMCCH"
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5 #5
  costmap_converter:
   CostmapToPolygonsDBSMCCH:
     cluster_max_distance: 0.3
     cluster_min_pts: 2
     cluster_max_pts: 30
     convex_hull_min_pt_separation: 0.1
    # CostmapToLinesDBSMCCH:
    #   cluster_max_distance: 0.4
    #   cluster_min_pts: 2
    #   cluster_max_pts: 30
    #   convex_hull_min_pt_separation: 0.1

  # Optimization Parameters
  no_inner_iterations: 4
  no_outer_iterations: 5 # trajetory resizing done with this iteration
  optimization_activate: true
  optimization_verbose: false
  penalty_epsilon: 0.01
  obstacle_cost_exponent: 1.0
  weight_max_vel_x: 2.0
  weight_max_vel_theta: 1.0 
  weight_acc_lim_x: 1.0 #1.0
  weight_acc_lim_theta: 1.0 #1.0
  weight_kinematics_nh: 1000.0
  weight_kinematics_forward_drive: 200.0 
  weight_kinematics_turning_radius: 0.0 
  weight_optimaltime: 10.0 #1.0 
  weight_obstacle: 75.0 #old50.0
  weight_dynamic_obstacle: 50.0
  weight_dynamic_obstacle_inflation: 0.1
  weight_viapoint: 1.0
  weight_inflation: 0.1
  weight_adapt_factor: 2.0
  weight_shortest_path: 1.0
  weight_velocity_obstacle_ratio: 1.0 # Default 0 Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle

  # Parallel Planning in distinctive Topologies
  enable_homotopy_class_planning: true 
  enable_multithreading: true
  max_number_classes: 2 # 2 classes are enough if we are just looking for left and right movement around obstacles
  selection_cost_hysteresis: 1.0
  selection_prefer_initial_plan: 0.95
  selection_obst_cost_scale: 2.0
  selection_viapoint_cost_scale: 1.0
  selection_alternative_time_cost: false
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  h_signature_prescaler: 1.0
  h_signature_threshold: 0.1
  obstacle_heading_threshold: 0.45
  visualize_hc_graph: false
  viapoints_all_candidates: false
  switching_blocking_period: 0.0
  delete_detours_backwards: true

  # Recovery
  shrink_horizon_backup: true
  shrink_horizon_min_duration: 10
  oscillation_recovery: true
  oscillation_v_eps: 0.1
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 10
  oscillation_filter_duration: 10

  #footprint_model: # types: "point", "circular", "line", "two_circles", "polygon"
  # type: "polygon"
  # vertices: [[0.88,0.42],[-0.29,0.42],[-0.29,-0.42],[0.88,-0.42]] # simple rectangle
  footprint_model:
   type: "line"
   line_start: [-0.2, 0.0]
   line_end: [0.88, 0.0]

  odom_topic: "/odom"
  map_frame: "/map"