Fix oscillations on v-shaped turning
Fix oscillations on v-shaped turning

Signed-off-by: Michał Antoszkiewicz <014e6a1472da4ef0cc84c4b0faa4febedb53d004@flyps.io>

rst-tu-dortmund/teb_local_planner,rst-tu-dortmund/teb_local_planner