Added a motion planer for the endline U turn. TODO test it with the ros turtle.