[ROS1 Kinetic] TEB generates unfeasible paths with very few poses

[kajep09]

Hi @croesmann

I am using the kinetic-devel version of the TEB planner and I have intermittent problems where it will generate a plan that only contains ~3 poses (according to the published pose array) that are spaced far apart and results in violent and unpredictable robot behavior. I included a video of the robot (mainly standing still so it doesn't go crazy) where I give it different goals through move_base and sometimes it generates a nice path, but sometimes it generates this extremely weird path.

https://youtu.be/9S-V2xUP_JA

Can you point me in the right direction with regards to which parameters that could be causing such a problem? Or is it a bug that I can't fix by playing with the parameters? I included the parameters I use below, but I have seen that kind of behavior quite a few times while playing around with various parameters.

base_local_planner: teb_local_planner/TebLocalPlannerROS
TebLocalPlannerROS:
  # Trajectory
  teb_autosize: true                       
  dt_ref: 0.3                              
  dt_hysteresis: 0.1                       
  global_plan_overwrite_orientation: false 
  allow_init_with_backwards_motion: false  
  max_global_plan_lookahead_dist: 3.0      
  force_reinit_new_goal_dist: 1.0          
  feasibility_check_no_poses: 5            
  global_plan_viapoint_sep: 0.25           
  via_points_ordered: false                
  exact_arc_length: false                  
  publish_feedback: true                   

  # Robot
  max_vel_x: 0.5                           
  max_vel_x_backwards: 0.11                
  max_vel_y: 0.0                           
  max_vel_theta: 0.5                       
  acc_lim_x: 1.5                           
  acc_lim_y: 0.0                           
  acc_lim_theta: 2.0                       
  min_turning_radius: 0.0                  
  wheelbase: 1.0                           
  cmd_angle_instead_rotvel: false          
  is_footprint_dynamic: false              
  footprint_model:
    type: "line"
    line_start: [-0.4, 0.0]
    line_end: [0.4, 0.0]

  # Goal tolerance
  xy_goal_tolerance: 0.05 #0.03            
  yaw_goal_tolerance: 0.1 #0.08            
  free_goal_vel: false                     

  # Obstacles
  min_obstacle_dist: 0.6                   
  inflation_dist: 0.1                      
  dynamic_obstacle_inflation_dist: 0.6     
  include_dynamic_obstacles: false         
  include_costmap_obstacles: true          
  legacy_obstacle_association: false       
  obstacle_association_force_inclusion_factor: 1.5  
  obstacle_association_cutoff_factor: 5.0           
  costmap_obstacles_behind_robot_dist: 1.5 
  obstacle_poses_affected: 10              

  # Optimization
  no_inner_iterations: 5                   
  no_outer_iterations: 4                   
  optimization_activate: true              
  optimization_verbose: false              
  penalty_epsilon: 0.1                     
  weight_max_vel_x: 2.0                    
  weight_max_vel_y: 2.0                    
  weight_max_vel_theta: 1.0                
  weight_acc_lim_x: 1.0                    
  weight_acc_lim_y: 1.0                    
  weight_acc_lim_theta: 1.0                
  weight_kinematics_nh: 1000.0             
  weight_kinematics_forward_drive: 1.0     
  weight_kinematics_turning_radius: 1.0    
  weight_optimaltime: 1.0                  
  weight_obstacle: 50.0                    
  weight_inflation: 0.1                    
  weight_dynamic_obstacle: 50.0            
  weight_dynamic_obstacle_inflation: 0.1   
  weight_viapoint: 1.0                     
  weight_adapt_factor: 2.0                 

  # Homotopy Class Planner
  enable_homotopy_class_planning: true    
  enable_multithreading: true             
#  simple_exploration: false              
  max_number_classes: 2                   
  selection_cost_hysteresis: 1.0          
  selection_prefer_initial_plan: 0.95     
  selection_obst_cost_scale: 100.0        
  selection_viapoint_cost_scale: 1.0      
  selection_alternative_time_cost: false  
  roadmap_graph_no_samples: 15            
  roadmap_graph_area_width: 5.0           
  roadmap_graph_area_length_scale: 1.0    
  h_signature_prescaler: 1.0              
  h_signature_threshold: 0.1              
#  obstacle_keypoint_offset: 0.1          
  obstacle_heading_threshold: 0.45        
  viapoints_all_candidates: true          
  visualize_hc_graph: false               
  visualize_with_time_as_z_axis_scale: 0.0 

  # Recovery
  shrink_horizon_backup: true              
  oscillation_recovery: true               

  odom_topic: odom_comb #odom
  map_frame: odom_comb #odom 

[08dipshikha]

HI. I have been going through some issues with teb itself. Coming across this question and looking at the parameters, one factor that could give you "infeasible paths with few poses" is feasibility_no_of_poses. This value defines the number of checks to look ahead if the path generated is feasible or not. Since the value is small here, infeasibility is checked with few poses itself. Although your path ahead looks clear and that is a doubt too but another set of params that you could consider would be the min_obstacle_dist and inflation_dist. First of all the inflation_dist should be a value greater than min_obstacle_dist according to http://wiki.ros.org/teb_local_planner. Also min_obstacle_dist should be a value that satisfies the fact that the width_of_robot + 2*min_obstacle_dist < corridor width. The robot will try to maintain from both sides the min_obstacle_dist and in few cases that will cause paths going backward and forward etc. Maybe give these a shot and see if there is any change in behavior.