(video) Path Planning Algorithm

The video shows path planning being done by the vehicle once it detects the obstacles in its surroundings.
On the left is a simulator showing a vehicle equipped with 2D laser range finder and a camera moving in an urban world.
On the right is the obstacles marked and inflated (orange color) to maintain minimum safe gap between vehicle and obstacle. The algorithm then plans a motion path from its current location to destination taking into account kinematic and dynamic constraints of the vehicle. Note the k-turn planned and executed by the vehicle to move out of the parking lane.
In the center, the obstacle field has been shown overlaid of the live camera feed for easy visualization and debugging.