re-arranged things in Assignment1 and made it show us things before it started making the poses
Nodes are now create at the mass center of ASVs (for the start and goal nodes)