Able to accept goalPose and create basic enviroment file
Able to accept goalPose and create basic enviroment file

IEEERobotics/high-level,IEEERobotics/high-level,IEEERobotics/high-level,IEEERobotics/high-level,IEEERobotics/high-level