prevent teb_local_planner from getting stuck while turning around obstacle (carlike)
I am running teb_local_planner in simulation for a carlike robot. Quite often I see teb_local_planner getting stuck while trying to turn around an obstacle. I guess this is because during the path generation/selection teb_local_planner thinks this path is totally fine, but right before sending the command it realizes the car actually cannot follow the path without getting too close to the obstacle
Here an example where the car is trying to do a 2 point turn to destination but stops close to the wall because the paths is not good, yet it cannot find any better path and simply waits here endlessly.:
I have tried to increase the size of the footprint in teb_local_planner (two_circles) so it is always bigger than the actual vehicle, but still the planner generates and selects paths that ends up being impossible.
What parameter could I adjust to solve this behavior? (reducing min_obstacle_dist is obviously not a solution)
EDIT: my teb_local_planner config:
acc_lim_theta: 0.5
acc_lim_x: 0.5
acc_lim_y: 0.5
allow_init_with_backwards_motion: true
cmd_angle_instead_rotvel: true
complete_global_plan: true
costmap_converter:
CostmapToPolygonsDBSConcaveHull:
cluster_max_distance: 2.0
cluster_max_pts: 200
cluster_min_pts: 2
concave_hull_depth: 10.0
convex_hull_min_pt_separation: 0.5
min_support_pts: 2
ransac_convert_outlier_pts: true
ransac_filter_remaining_outlier_pts: false
ransac_inlier_distance: 1.0
ransac_min_inliers: 10
ransac_no_iterations: 2000
ransac_remainig_outliers: 3
support_pts_max_dist: 3.0
support_pts_max_dist_inbetween: 6.0
costmap_converter_plugin: costmap_converter::CostmapToPolygonsDBSConcaveHull
costmap_converter_rate: 5
costmap_converter_spin_thread: true
costmap_obstacles_behind_robot_dist: 5.0
delete_detours_backwards: false
divergence_detection_enable: false
divergence_detection_max_chi_squared: 10.0
dt_hysteresis: 0.1
dt_ref: 0.3
dynamic_obstacle_inflation_dist: 8.0
enable_homotopy_class_planning: true
enable_multithreading: true
exact_arc_length: false
feasibility_check_no_poses: 5
footprint_model:
front_offset: 6.0
front_radius: 8.0
rear_offset: 0.0
rear_radius: 8.0
type: two_circles
force_reinit_new_goal_angular: 0.78
force_reinit_new_goal_dist: 1.0
free_goal_vel: false
gear_change_time: 2.0
global_plan_overwrite_orientation: true
global_plan_prune_distance: 1
global_plan_viapoint_sep: -0.1
h_signature_prescaler: 0.75
h_signature_threshold: 0.1
include_costmap_obstacles: true
include_dynamic_obstacles: true
inflation_dist: 8.0
is_footprint_dynamic: false
legacy_obstacle_association: false
max_global_plan_lookahead_dist: 200.0
max_number_classes: 10
max_number_plans_in_current_class: 10
max_ratio_detours_duration_best_duration: 3.0
max_samples: 500
max_vel_theta: 100.0
max_vel_x: 3.78
max_vel_x_backwards: 3.78
max_vel_y: 0.0
min_obstacle_dist: 4.0
min_turning_radius: 20.0
no_inner_iterations: 5
no_outer_iterations: 4
obstacle_association_cutoff_factor: 5.0
obstacle_association_force_inclusion_factor: 1.5
obstacle_cost_exponent: 4.0
obstacle_heading_threshold: 0.45
obstacle_poses_affected: 15
obstacle_proximity_lower_bound: 0.0
obstacle_proximity_ratio_max_vel: 1.0
obstacle_proximity_upper_bound: 0.5
odom_topic: odom
optimization_activate: true
optimization_verbose: false
oscillation_filter_duration: 4
oscillation_omega_eps: 0.2
oscillation_recovery: false
oscillation_recovery_min_duration: 4
oscillation_v_eps: 1.0
penalty_epsilon: 0.1
publish_feedback: false
roadmap_graph_area_length_scale: 2.0
roadmap_graph_area_width: 60.0
roadmap_graph_no_samples: 45
selection_alternative_time_cost: false
selection_cost_hysteresis: 0.95
selection_dropping_probability: 0.05
selection_obst_cost_scale: 2.0
selection_prefer_initial_plan: 1.0
selection_viapoint_cost_scale: 1.0
shrink_horizon_backup: false
shrink_horizon_min_duration: 10
switching_blocking_period: 2.0
teb_autosize: true
theta_stopped_vel: 0.1
trans_stopped_vel: 0.1
transform_tolerance: 0.5
use_proportional_saturation: false
via_points_ordered: false
viapoints_all_candidates: true
visualize_hc_graph: false
visualize_with_time_as_z_axis_scale: 0.0
weight_acc_lim_theta: 1.0
weight_acc_lim_x: 1.0
weight_acc_lim_y: 1.0
weight_adapt_factor: 2.0
weight_dynamic_obstacle: 50.0
weight_dynamic_obstacle_inflation: 0.1
weight_gear_change: 50.0
weight_inflation: 0.1
weight_kinematics_forward_drive: 1.0
weight_kinematics_goal_orientation: 50.0
weight_kinematics_nh: 1000.0
weight_kinematics_turning_radius: 10.0
weight_max_vel_theta: 1.0
weight_max_vel_x: 1.0
weight_max_vel_y: 2.0
weight_obstacle: 50.0
weight_optimaltime: 10.0
weight_shortest_path: 10.0
weight_velocity_obstacle_ratio: 0.0
weight_viapoint: 1.0
wheelbase: 6.5
xy_goal_tolerance: 0.5
yaw_goal_tolerance: 0.3
What's your
penalty_epsilon
parameter? Can you display all parametersThank you what range of
minimum turning radius
have you tried. And forpenalty epsilon
try to play with small increments but 0.1 is default parameter.I have tried multiple values for min_turning_radius from 20 to 30. The actual vehicle I want to model has a pretty big turning radius so I don't want to go lower than 20. It seems indeed that increasing
penalty_epsilon
helps. A value around 1.4~1.5 seems to solve my issue. Thanks!Great I’ll change to answer later today. Glad it helped