Rosanswers logo

I am testing teb_local_planner for a carlike vehicle. I am trying to make the robot park backward, for which I would expect the planner to turn in 2-3 times (as a human driver would). However the local planner keeps generating and selecting trajectories that are just impossible for the car to follow (the min_turning_radius is set to 10 meters)

For example here, the selected red path requires the vehicle to make a sharp U-turn, that should not be possible with my min_turning_radius. As the vehicle approaches and fails to follow the local path, the local planner keeps generating more and more complex and impossible paths.

How come the planner can generate such impossible plan?

this trajectory should be impossible

That said, it is not always the case. Sometimes, when I am lucky, the planner manages to generate a very nice 2-way turn curve (like in the picture below).

this is better

What parameters could I adjust to make the planner consistently generate (and select) paths like this instead?


Originally posted by vrichard on ROS Answers with karma: 108 on 2021-10-14

Post score: 0

Original comments

Comment by Dragonslayer on 2021-10-14:
Orientation maybe? Its hard to see, but whats the goal orientation? Also your painted arrows seem to suggest that the robot should reverse into position. Would be important to see your config file for the planner. Do those "strange" paths pass the feasability check or do they get errored out?

Comment by vrichard on 2021-10-14:
Yes the goal and start points are facing each other (see https://imgur.com/a/M0tvaqi). So the vehicle has to park in reverse (or find a way to turn around at some point). For some reason the planner always prefer to reach the goal point first and then to try to turn around. As you can see with the walls near by the maneuver is not very wise from that point. All the impracticable paths pass the feasibility check and I never get any error.


1 Answer 1


Rosanswers logo

After all, I might be caused by the size of my vehicle and the fact the goal is too close to the wall for teb_local_planner to find any good maneuver.

I have just tested the same situation but with a smaller vehicle: I have simply reduced footprint and wheelbase parameters by a factor 2. And now it works rather nice:

image description image description image description

So maybe I can solve my problem just by moving the goal point a bit further away from the wall

EDIT: making it work for bigger robots was harder than I thought. There is not just the distance to the wall that matters. Actually even without obstacles around the planner was unable to find good turn around strategies for the bigger vehicle. It always ended up making in-place star shaped turns like this: inefficient turn

I went through a lot tuning and still could not make it work properly. So I ended up implementing 2 extras cost functions to the planner:

  • one that introduces a penalty when the vehicle changes from driving forward to driving backward (I described here) to encourage the planner to find 2-3 point turns instead of star shape plans like above
  • another which introduces a penalty for not being oriented the same than the local planner goal. Basically the idea with this one is to force the planner to orient the vehicle properly as soon as possible. This fixed the previous behavior I had most of the time: the planner would first want to reach destination position and from there would start to wonder how to turn around (and fail miserably at it)

I have not tested much the individual contribution of each new penalty but the conjunction of both works rather nicely.

I settled my weights like this:

  • weight_kinematics_nh: 1000
  • weight_same_orientation_than_goal, weight_forward_backward_switch, weight_optimal_time, weight_obstacle, weight_dynamic_obstacle : 50
  • weight_kinematics_turning_radius: 10
  • other weights: default

As for the Homotopy class planning I increased the area width, scale and number of samples. I also reduced h_signature_prescaler to 0.75 which looks a bit magic to me but had a positive effect on the quality of the planner generated paths. In order to make this run fast enough, I had to reduce a bit the size and resolution of my costmap. Most other parameters are defaults (or just scaled to the size of the robot)

Originally posted by vrichard with karma: 108 on 2021-10-14

This answer was ACCEPTED on the original site

Post score: 2

Original comments

Comment by Dragonslayer on 2021-10-15:
To get a clean solution the planner should error out though. If the problem is indeed the distance to the wall (collision), increasing the costmap footprint will lead to the feasibility check failing, as far as I understand it. See this "For detecting if a collision occurs, the costmap footprint is used." Or using a polygon to match the "real" footprint might work as well, high cpu cost though. This is a really interessting problem, specially the planner planning 90° turns. Would really like to see that config.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.