Initial work on grid-based local planner