Fixed get_next_goal to publish actual coordinates instead of cell indexes