ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

TEB local planner issue on narrow corridors/doors

asked 2022-03-03 10:41:16 -0600

thibs-sigma gravatar image

updated 2022-03-03 10:43:21 -0600

Hi,

I'm facing an issue using the (wonderful) teb_local_planner. I'm trying to solve this since a few weeks now, but I can't figure out how to do it...

Here is the problem: I'm trying to pass through really narrow corridors and doors, which are about 1.3m width with a rectangular robot about 0.5m width, So basically 40cm from each side.

The global plan (light line) seems correct and pass through the door right in the middle (see picture below). However, the local plan (strong line) is really close to the obstacle zone, and TEB pose checking can't solve it (red rectangle). PICTURE HERE

I tried setting the min_obstacle_distance to 0.7, and here, the local plan aligns with the global plan predicition : OTHER PICTURE HERE.

But sometimes, it still gets stuck. One simple solution would be to go backwards a little, and realign with the door, but TEB doesn't seem to like it.

Am I adjusting the right parameters? Or is there something else, especially in the Optimization section of TEB's parameters, I could tune?

Here are my param files :

TEB Local Planner

#ns : TebLocalPlannerROS:

# Misc
odom_topic      : odom
footprint_model:         
  type          : "line"                                                                                                            
  radius        : 0.2                                                             
  line_start    : [-0.15, 0.0]                                                   
  line_end      : [0.15, 0.0]                                                     
  front_offset  : 0.2                                                            
  front_radius  : 0.2                                                            
  rear_offset   : 0.2                                                           
  rear_radius   : 0.2                                                           
  vertices      : [ [-0.31, -0.25], [-0.31, 0.25], [0.31, 0.25], [0.31, -0.25] ] 


# GoalTolerance
complete_global_plan: False  
trans_stopped_vel   : 0.05  
theta_stopped_vel   : 0.4  
xy_goal_tolerance   : 0.3   
yaw_goal_tolerance  : 0.3  
free_goal_vel       : False 


# HCPlanning (Homotopy Class Planner)
delete_detours_backwards                  : True        
detours_orientation_tolerance             : M_PI / 2.0  
max_ratio_detours_duration_best_duration  : 3.0        
enable_homotopy_class_planning            : True        
simple_exploration                        : False      
length_start_orientation_vector           : 0.4        
enable_multithreading                     : True        
max_number_classes                        : 4      
max_number_plans_in_current_class         : 1         
selection_cost_hysteresis                 : 1.0       
selection_prefer_initial_plan             : 0.9        
selection_obst_cost_scale                 : 100.0       
selection_alternative_time_cost           : False     
selection_dropping_probability            : 0.0        
switching_blocking_period                 : 0.0        
roadmap_graph_no_samples                  : 15         
roadmap_graph_area_width                  : 5 
roadmap_graph_area_length_scale           : 1.0         
h_signature_prescaler                     : 0.5        
h_signature_threshold                     : 0.1       
obstacle_heading_threshold                : 0.45       
obstacle_keypoint_offset                  : 0.1      
viapoints_all_candidates                  : True       
visualize_hc_graph                        : False   


# Obstacles
costmap_converter_plugin                    : "costmap_converter::CostmapToLinesDBSRANSAC" 
costmap_converter_spin_thread               : True  #
costmap_converter_rate                      : 5     
costmap_converter/CostmapToLinesDBSRANSAC:
  cluster_max_distance                  : 0.4
  cluster_min_pts                       : 2
  ransac_inlier_distance                : 0.15   
  ransac_min_inliers                    : 10      
  ransac_no_iterations                  : 1500    
  ransac_remainig_outliers              : 3      
  ransac_convert_outlier_pts            : True    
  ransac_filter_remaining_outlier_pts   : False   
  convex_hull_min_pt_separation         : 0.1
min_obstacle_dist                           : 0.01 
inflation_dist                              : 0.02   
dynamic_obstacle_inflation_dist             : 0.6   
include_dynamic_obstacles                   : True  
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                     : 15  
## Reduce_velocity_near_obstacles
obstacle_proximity_ratio_max_vel            : 1.0  
obstacle_proximity_lower_bound              : 0.0  
obstacle_proximity_upper_bound              : 0.5  


# Optimization
weight_prefer_rotdir              : 1.0   
### Dynamically reconfigurable
no_inner_iterations               : 5      
no_outer_iterations               : 4       
optimization_activate             : True   
optimization_verbose              : False 
penalty_epsilon                   : 0.0     
weight_max_vel_x                  : 2     
weight_max_vel_y                  : 2      
weight_max_vel_theta              : 1       
weight_acc_lim_x                  : 1       
weight_acc_lim_y                  : 1      
weight_acc_lim_theta              : 1       
weight_kinematics_nh              : 1000    
weight_kinematics_forward_drive   : 100     
weight_kinematics_turning_radius  : 100    
weight_optimaltime                : 1.0    
weight_shortest_path              : 100.0   
weight_obstacle                   : 100     
weight_inflation                  : 0.2     
weight_dynamic_obstacle           : 10      
weight_dynamic_obstacle_inflation : 0.2    
weight_velocity_obstacle_ratio    : 0.0     
weight_viapoint                   : 1      
weight_adapt_factor               : 2      
obstacle_cost_exponent            : 4    


# Recovery
divergence_detection_enable           : True  
divergence_detection_max_chi_squared  : 0     
oscillation_filter_duration           : 2.0   
oscillation_recovery_min_duration     : 2.0   
oscillation_v_eps                     : 0.1   
oscillation_omega_eps                 : 0.1   
shrink_horizon_min_duration           : 3.0   
### Dynamically reconfigurable
shrink_horizon_backup                 : True 
oscillation_recovery                  : True  


# Robot
### Dynamically reconfigurable
max_vel_x                   : 0.3   
max_vel_x_backwards         : 0.3  
max_vel_theta               : 1.5      
acc_lim_x                   : 1.5  
acc_lim_theta               : 25.0  
is_footprint_dynamic        : False
use_proportional_saturation : True ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-03-04 10:42:50 -0600

Scotty gravatar image

Hi thibs, I've encountered something similar to this issue myself. The thing about teb planner is that it doesn't consider the inflation layer like the global planner does it's more concerned with the lethal cost of a maps boundaries (teb_tutorial).

The ways I tuned for areas with narrower free space is by using tebs in built inflation calculation [inflation_dist] [weight_inflation], [feasibility_check_no_poses] which checks against the foot print along the current teb poses for viability, and [global_plan_viapoint_sep] which sets points along the global plan that teb tries to stay close to. If you have some processing power to spare I would also recommend switching from line to "two circles" as your footprint, this should help with the pose calculations

In terms of moving slightly backward when the robot is stuck, that's better handled by a recovery behavior.

edit flag offensive delete link more

Comments

Hi Scotty, thanks for your answer! I tried with a two_cricles model, that seems a bit better ('polygon' requires too much ressources on my platform). Thanks for the suggestion on recovery behaviors, I'm gonna write my own one for that. It's working, but not still satisfying, with these parameters :

min_obstacle_dist: 0.7 # if I set it smaller, the robot can't pass the door
inflation_dist: 0.2 # I don't really see any influcence
weight_inflation : 10.0
feasibility_check_no_poses: 15
global_viapoint_sep: 0.15

Do you mind sharing your set of parameters? I'm still troubling on finding the right ones. Many thanks!

thibs-sigma gravatar image thibs-sigma  ( 2022-03-07 04:08:49 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2022-03-03 10:41:16 -0600

Seen: 180 times

Last updated: Mar 03