At the moment, we know geometry of the road and the starting/collision waypoints. Next step we will keeping these initial waypoints and generating few waypoints between them in order to creating trajectory for the vehicle and avoiding the usage of angle.
Trajectory should be cubic-splines and references to the main street. All waypoints must not be placed out of the street.