TEB local planner issue on narrow corridors/doors
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 ...
