teb_local_planner having difficulty making a 90 degree turn

HI All I have applied the teb_local_planner to my move_base for for my physical car like robot.. It reaches the goals i set successfully in my living room when just travelling in a straight line is required, or just soft turns are required to reach the goal pose

However when bigger turns are required the planner seems to abandon the initial feasible plan to make the turn and adopt more and more complex turn sequences, without giving the original plan a chance to work. It claims the trajectory is not feasible, but i dont understand why. I have a link to a youtube video of what happens

Im sure i have mis-configured it. But can anyonepoint me the direction what parameters i should be looking at? here is my base_local_planner_params.yaml

TebLocalPlannerROS:

odom_topic: odom

# Trajectory

teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
max_samples: 500
global_plan_overwrite_orientation: True
allow_init_with_backwards_motion: True
global_plan_viapoint_sep: -1
global_plan_prune_distance: 1
exact_arc_length: False
feasibility_check_no_poses: 2
publish_feedback: True

# Robot

max_vel_x: 0.18
max_vel_x_backwards: 0.18
max_vel_y: 0.0
max_vel_theta: 0.5 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
acc_lim_x: 0.5
acc_lim_theta: 0.7

# ********************** Carlike robot parameters ********************
min_turning_radius: 2.0        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
wheelbase: 0.3                 # Wheelbase of our robot
cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
# ********************************************************************

footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "two_circles"
radius: 0.2 # for type "circular"
line_start: [0.0, 0.0] # for type "line"
line_end: [0.4, 0.0] # for type "line"
front_offset: 0.3 # for type "two_circles"
front_radius: 0.2 # for type "two_circles"
rear_offset: 0.0 # for type "two_circles"
rear_radius: 0.2 # for type "two_circles"
vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

# GoalTolerance

xy_goal_tolerance: 0.4
yaw_goal_tolerance: 0.25
free_goal_vel: False
complete_global_plan: True

# Obstacles

min_obstacle_dist: 0.27 # This value must also include our robot's expansion, since footprint_model is set to "line".
inflation_dist: 0.1
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 15

dynamic_obstacle_inflation_dist: 0.6
include_dynamic_obstacles: True

costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
costmap_converter_rate: 5

# Optimization

no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
obstacle_cost_exponent: 4
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: 1
weight_optimaltime: 1 # must be > 0
weight_shortest_path: 0
weight_obstacle: 100
weight_inflation: 0.2
weight_dynamic_obstacle: 10 # not in use yet
weight_dynamic_obstacle_inflation: 0.2
weight_viapoint: 1

# Homotopy Class Planner

enable_homotopy_class_planning: True
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_prefer_initial_plan: 0.95
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False

h_signature_prescaler: 0.5
h_signature_threshold: 0.1
switching_blocking_period: 0.0
viapoints_all_candidates: True
delete_detours_backwards: True
max_ratio_detours_duration_best_duration: 3.0
visualize_hc_graph: False
visualize_with_time_as_z_axis_scale: False

# Recovery

shrink_horizon_backup: True
shrink_horizon_min_duration ...
edit retag close merge delete

Sort by » oldest newest most voted

It looks like you are running SLAM at the same time as navigation. This is not always a good idea since it can mess up the paths the local planner i.e. TEB is trying to generate because

1. the map area you are sending it to has unexplored areas and may be avoided by the planner because of cost
2. the map is changing as the robot is moving and SLAM is running, which can instantiate a new local plan being generated.

Try running it with a static map and see if you still experience the same behavior.

TEB also requires a lot of tuning, here is one example of TEB running on an omni-directional mobile robot in a static map https://youtu.be/VfQWa06hQlM. It takes a lot of tuning, and it is hard to say it just one specific parameter. I can try and make one for a car-like robot and see how it behaves.

more

min_turning_radius: 2.0 is probably wrong. It indicates to the planner that your car is not able to make turns with a radius of less than two meters. I don't know how large your living room is, but I suspect your robot is small enough where its minimum turning radius is much less than that.

more