Some minor changes - fixing a nullpointer exception when the number of obstacles in graph
decreased number of nodes and thus the original goal had too high ID.