First version of a controller for automatic driving.
Fixed one bug in local path iterator.
Testing headings in local planner test, smoother curves to get better precision in test.
Test refactoring.