High level decision making and path planning