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