Added ROS flight primitive code as it currently is, including older version of straight line (does not work particularly well). Still working on cleaning up and fixing chunks of the code