Planning ready for real world
Writing a trajectory file