Implements stepsTaken to generate steps taken from start to goal node