Configure teb_local_planner

asked 2021-10-12 03:48:46 -0600

zuy gravatar image

updated 2022-05-01 13:08:14 -0600

lucasw gravatar image

Hi, I have a diff-drive robot and I'm new to teb_local_planner. I found that teb_local_planner is actually hard to be properly configured. My robot couldn't find a feasible trajectory even though the goal is quite easy to reach and there was no obstacle. Maybe I did something wrong in the configuration file, please take a look and give me some advice. Thank you!

TebLocalPlannerROS:
 odom_topic: odom
 map_frame: map

# Robot
acc_lim_x: 0.5
acc_lim_theta: 0.5
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_vel_theta: 0.3
min_turning_radius: 0.0
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
 type: "polygon"
 vertices: [[-0.09, -0.26], [-0.09, 0.26], [0.8, 0.26], [0.8, -0.26]] # for type "polygon"
is_footprint_dynamic: True

# GoalTolerance

xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.1
free_goal_vel: False

# Trajectory

# teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
min_samples: 5
global_plan_overwrite_orientation: True
global_plan_viapoint_sep: 0.1
max_global_plan_lookahead_dist: 3.0
force_reinit_new_goal_dist: 0.5
feasibility_check_no_poses: 5
publish_feedback: False
shrink_horizon_backup: True
allow_init_with_backwards_motion: False
exact_arc_length: False
shrink_horizon_min_duration: 10.0

# Obstacles

min_obstacle_dist: 0.4
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 30
inflation_dist: 0.5
include_dynamic_obstacles: False
legacy_obstacle_association: False
obstacle_association_force_inclusion_factor: 1.5
obstacle_association_cutoff_factor: 5
# costmap_converter_plugin: ""
# costmap_converter_spin_thread: True
# costmap_converter_rate: 5

# Optimization

no_inner_iterations: 5
no_outer_iterations: 4
penalty_epsilon: 0.1
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 1000
# weight_kinematics_turning_radius: 1
weight_optimaltime: 1
weight_obstacle: 50
weight_viapoint: 1.0
weight_inflation: 0.1
weight_adapt_factor: 2.0
# optimization_activate: True
# optimization_verbose: False

# weight_dynamic_obstacle: 10 # not in use yet

# Homotopy Class Planner

enable_homotopy_class_planning: True
enable_multithreading: True
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 100.0
selection_viapoint_cost_scale: 1.0
selection_alternative_time_cost: False
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_heading_threshold: 0.45
visualize_hc_graph: False
viapoints_all_candidates: True
switching_blocking_period: 0.0
# simple_exploration: False

# obstacle_keypoint_offset: 0.1

Also, I'm using GlobalPlanner as my global planner:

GlobalPlanner:
 visualize_potential: true
 cost_factor: 0.55
 neutral_cost: 66
 lethal_cost: 253
 publish_potential: True
 orientation_mode: 0

My terminal:

[ WARN] [1634027051.331745999]: Control loop missed its desired rate of 1.0000Hz... the loop actually took 2.5395 seconds
[ WARN] [1634027051.332600445]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 2.3909 seconds
[ WARN] [1634027056.209614701]: Control loop missed its desired rate of 1.0000Hz... the loop actually took 4.8779 seconds
[ WARN] [1634027056.210423834]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 4.8778 seconds
[ WARN] [1634027060.063637919]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
edit retag flag offensive close merge delete

Comments

I would first try to get rid of the timing issues. Here specially the footprint model might be of interesst (double circles = less cpu). teb tutorials Also, as stated on the linked page, the feasibility is checked against the costmap's footprint model. Maybe some wrong data there? In addition to that, the paramter "global_plan_viapoint_sep: 0.1" might be a problem. In demos its more like 1.0. Might "pull" the robot into obstacles? Might be worth a try to disable it completely (set -0.1).

Dragonslayer gravatar image Dragonslayer  ( 2021-10-12 07:42:57 -0600 )edit