astar with P-controller on steering tracking Dubins curves, planner does not check map yet