teb_local_planner keeps pushing the vehicle turning point toward start position during optimization
I am using a slightly modified version of test_optim_node.cpp
to tune teb_local_planner
with a car-like vehicle.
I have set the start point on the right side of the screen and the goal point on the left side of the screen. Both poses are facing each other, so that the teb_local_planner
needs to figure a path to turn the vehicle around before the goal point is reached. Up to here the planner is doing rather fine.
However the curious part is that during the optimization process, teb_local_planner
slowly moves the turning point closer and closer toward the start point. This absolutely makes no sense to me, since I have set max_vel_x
rather high compared to max_vel_x_backwards
so the weight_optimaltime
should push teb_local_planner
to move the turn point closer to the goal position. I can confim with visualize_with_time_as_z_axis_scale
that moving the turning point closer to the start position increases significantly the planned travel time.
Is there any weight/param that makes the vehicle prefer to turn as early as possible and compensate the time lost going backward?