Added constructions of goal and navigation functions