Ask Your Question
0

prevent teb_local_planner from getting stuck while turning around obstacle (carlike)

asked 2021-10-19 01:30:45 -0600

vrichard gravatar image

updated 2021-10-19 18:36:42 -0600

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.:

image description

image description

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
edit retag flag offensive close merge delete

Comments

1

What's your penalty_epsilon parameter? Can you display all parameters

osilva gravatar image osilva  ( 2021-10-19 13:28:58 -0600 )edit
1

Thank you what range of minimum turning radius have you tried. And for penalty epsilon try to play with small increments but 0.1 is default parameter.

osilva gravatar image osilva  ( 2021-10-19 19:36:41 -0600 )edit

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!

vrichard gravatar image vrichard  ( 2021-10-19 20:43:29 -0600 )edit

Great I’ll change to answer later today. Glad it helped

osilva gravatar image osilva  ( 2021-10-20 03:43:40 -0600 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2021-10-20 12:59:09 -0600

One of the parameters for optimization that has considerable impact is penalty_epsilon.

As per documentation: http://wiki.ros.org/teb_local_planner

<name>/penalty_epsilon (double, default: 0.1)
Add a small safety margin to penalty functions for hard-constraint approximations

Teb local planner tends to get stuck is local minimum, by slightly incrementing this value, the behaviour may be avoided.

An excellent resource: https://wiki.ros.org/teb_local_planne...

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-10-19 01:30:45 -0600

Seen: 34 times

Last updated: Oct 20