Configure teb_local_planner
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...
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).