Merged combined into master. Now planning to destination or full path planning is possible
Updated partial path planning with the centering functionality