Robotics StackExchange | Archived questions

TEB local planner issue on narrow corridors/doors

Hi,

I'm facing an issue using the (wonderful) teblocalplanner. 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 
transform_tolerance         : 0.5   
## Carlike
min_turning_radius          : 0.15  
wheelbase                   : 1.0   
cmd_angle_instead_rotvel    : False 
## Omnidirectional
max_vel_y                   : 0.0   
acc_lim_y                   : 0     
max_vel_trans               : 0.4   


# Trajectory
max_samples                           : 500   
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        : 3.0   
force_reinit_new_goal_dist            : 1.0  
force_reinit_new_goal_angular         : 0.78  
feasibility_check_no_poses            : 15    
feasibility_check_lookahead_distance  : -1.0  
exact_arc_length                      : True  
publish_feedback                      : True  
control_look_ahead_poses              : 1     
prevent_look_ahead_poses_near_goal    : 2     
visualize_with_time_as_z_axis_scale   : False

# ViaPoints
global_plan_prune_distance  : 1.0   
### Dynamically reconfigurable
global_plan_viapoint_sep    : 0.25 
via_points_ordered          : False

Local costmap

# ns : local_costmap
global_frame: map
robot_base_frame: base_link
footprint: [[-0.31, -0.22], [-0.31, 0.22], [0.31, 0.22], [0.31, -0.22]]

map_type: costmap
transform_tolerance: 0.5
update_frequency: 10.0
publish_frequency: 1.0
rolling_window: true
static_map: false
always_send_full_costmap: false

plugins:
  - {name: static_layer,            type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

width: 4
height: 4
resolution: 0.05
origin_x: 0.0
origin_y: 0.0

inflation_layer:
  inflation_radius: 1.45
  cost_scaling_factor: 10.0

obstacle_layer:
  max_obstacle_height: 2.0
  obstacle_range: 3.0
  raytrace_range: 4.0
  track_unknown_space: true
  footprint_clearing_enabled: true
  observation_sources: scan virtual_wall

  scan:
    topic: /sick_safetyscanners/scan 
    sensor_frame: ""
    observation_persistence: 0.0system latency. (double, default: 0.0)
    expected_update_rate: 0.0
    data_type: LaserScan
    clearing: true
    marking: true
    max_obstacle_height: 2.0
    min_obstacle_height: 0.0
    obstacle_range: 3.0
    raytrace_range: 4.0
    inf_is_valid: true

Global Costmap

# ns : global_costmap
global_frame: map
robot_base_frame: base_link
footprint: [[-0.31, -0.25], [-0.31, 0.25], [0.31, 0.25], [0.31, -0.25]]
map_type: costmap

transform_tolerance: 0.5
update_frequency: 1 
publish_frequency: 1
rolling_window: false
static_map: true
always_send_full_costmap: false

plugins:
  - {name: static_layer,            type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}  

width: 10
height: 10
resolution: 0.05
origin_x: 0.0
origin_y: 0.0

static_layer:
  unknown_cost_value: -1
  lethal_cost_threshold: 100
  map_topic: map
  first_map_only: false
  subscribe_to_updates: false
  track_unknown_space: true
  use_maximum: false
  trinary_costmap: true

inflation_layer:
  inflation_radius: 1.45
  cost_scaling_factor: 10.0 #0.1 --- 5.0

obstacle_layer:
  max_obstacle_height: 2.0
  obstacle_range: 3.0
  raytrace_range: 4.0
  track_unknown_space: true
  observation_sources: scan virtual_wall

  scan:
    topic: /sick_safetyscanners/scan
    sensor_frame: ""
    observation_persistence: 0.0
    expected_update_rate: 0.0
    data_type: LaserScan
    clearing: true
    marking: true(double, default: 2.0)
    max_obstacle_height: 2.0
    min_obstacle_height: 0.0
    obstacle_range: 3.0
    raytrace_range: 4.0
    inf_is_valid: true

I have a feeling that maybe is should set the allow_init_with_backwards_motion to True, but as I don't have (yet) sensors on the back of the robot, I would like if possible to avoid backwards motion... but only go backwards "a bit" in order to unstuck the robot only.

Many thanks for your help!

Cheers

Asked by thibs-sigma on 2022-03-03 11:41:16 UTC

Comments

Answers

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.

Asked by Scotty on 2022-03-04 11:42:50 UTC

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!

Asked by thibs-sigma on 2022-03-07 05:08:49 UTC