Added STOP mission option, fixed use of primitive end points
Disabled EKF absolute
Merge branch 'reorganization_v5' of https://github.com/labust/labust-ros-pkg into reorganization_v5