Add covariance info to Pose and Vel

Fix getMode bug

include/create/create.h

@@ -33,6 +33,7 @@ POSSIBILITY OF SUCH DAMAGE.
 #define CREATE_H
 
 #include <boost/shared_ptr.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
 #include <string>
 #include <unistd.h>
 
@@ -44,6 +45,8 @@ POSSIBILITY OF SUCH DAMAGE.
 namespace create {
   class Create {
     private:
+      typedef boost::numeric::ublas::matrix<float> Matrix;
+
       enum CreateLED {
         LED_DEBRIS = 1,
         LED_SPOT = 2,
@@ -75,9 +78,13 @@ namespace create {
       bool firstOnData;
       util::timestamp_t prevOnDataTime;
 
+      Matrix poseCovar;
+
       void init();
-      bool updateLEDs();
+      // Add two matrices and handle overflow case
+      Matrix addMatrices(const Matrix &A, const Matrix &B) const;
       void onData();
+      bool updateLEDs();
 
     protected:
       boost::shared_ptr<create::Data> data;

include/create/types.h

@@ -153,10 +153,10 @@ namespace create {
   };
 
   enum CreateMode {
-    MODE_OFF = OC_POWER,
-    MODE_PASSIVE = OC_START,
-    MODE_SAFE = OC_SAFE,
-    MODE_FULL = OC_FULL,
+    MODE_OFF = 0,
+    MODE_PASSIVE = 1,
+    MODE_SAFE = 2,
+    MODE_FULL = 3,
     MODE_UNAVAILABLE = -1
   };
 
@@ -226,6 +226,7 @@ namespace create {
     float x;
     float y;
     float yaw;
+    float covariance[9];
   };
 
   typedef Pose Vel;

include/create/util.h

@@ -69,6 +69,10 @@ namespace create {
       gettimeofday(&now, NULL);
       return now.tv_usec + (timestamp_t) now.tv_sec * 1000000;
     }
+
+    inline bool willFloatOverflow(const float a, const float b) {
+      return ( (a < 0.0) == (b < 0.0) && std::abs(b) > std::numeric_limits<float>::max() - std::abs(a) );
+    }
   }  // namespace util
 } // namespace create