Add covariance info to Pose and Vel

Fix getMode bug

examples/odom_example.cpp

@@ -27,17 +27,31 @@ int main(int argc, char** argv) {
 
   usleep(1000000);
 
-  // drive in a circle
+  // Drive in a circle
   robot->drive(0.1, 0.5);
 
   // Quit when center "Clean" button pressed
   while (!robot->isCleanButtonPressed()) {
     create::Pose pose = robot->getPose();
+    create::Vel vel = robot->getVel();
 
+    // Print pose
     std::cout << "x: " << pose.x
               << "\ty: " << pose.y
-              << "\tyaw: " << pose.yaw * 180.0/create::util::PI << std::endl;
+              << "\tyaw: " << pose.yaw * 180.0/create::util::PI << std::endl << std::endl;
   
+    // Print velocity
+    std::cout << "vx: " << vel.x
+              << "\tvy: " << vel.y
+              << "\tvyaw: " << vel.yaw * 180.0/create::util::PI << std::endl << std::endl;
+
+    // Print covariances
+    std::cout << "[ " << pose.covariance[0] << ", " << pose.covariance[1] << ", " << pose.covariance[2] << std::endl
+              << "  " << pose.covariance[3] << ", " << pose.covariance[4] << ", " << pose.covariance[5] << std::endl
+              << "  " << pose.covariance[6] << ", " << pose.covariance[7] << ", " << pose.covariance[8] << " ]" << std::endl << std::endl;;
+    std::cout << "[ " << vel.covariance[0] << ", " << vel.covariance[1] << ", " << vel.covariance[2] << std::endl
+              << "  " << vel.covariance[3] << ", " << vel.covariance[4] << ", " << vel.covariance[5] << std::endl
+              << "  " << vel.covariance[6] << ", " << vel.covariance[7] << ", " << vel.covariance[8] << " ]" << std::endl << std::endl;;
     usleep(1000 * 100); //10hz
   }
 

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_OFF = 0,
-    MODE_PASSIVE = OC_START,
+    MODE_PASSIVE = 1,
-    MODE_SAFE = OC_SAFE,
+    MODE_SAFE = 2,
-    MODE_FULL = OC_FULL,
+    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
 

src/create.cpp

@@ -12,6 +12,8 @@
 
 namespace create {
 
+  namespace ublas = boost::numeric::ublas;
+
   // TODO: Handle SIGINT to do clean disconnect
 
   void Create::init() {
@@ -33,6 +35,7 @@ namespace create {
     vel.x = 0;
     vel.y = 0;
     vel.yaw = 0;
+    poseCovar = Matrix(3, 3);
     data = boost::shared_ptr<Data>(new Data(model));
     serial = boost::make_shared<Serial>(data);
   }
@@ -50,6 +53,30 @@ namespace create {
     disconnect();
   }
 
+  Create::Matrix Create::addMatrices(const Matrix &A, const Matrix &B) const {
+    int rows = A.size1();
+    int cols = A.size2();
+
+    assert(rows == B.size1());
+    assert(cols == B.size2());
+
+    Matrix C(rows, cols);
+    for (int i = 0; i < rows; i++) {
+      for (int j = 0; j < cols; j++) {
+        const float a = A(i, j);
+        const float b = B(i, j);
+        if (util::willFloatOverflow(a, b)) {
+          // If overflow, set to float min or max depending on direction of overflow
+          C(i, j) = (a < 0.0) ? std::numeric_limits<float>::min() : std::numeric_limits<float>::max();
+        }
+        else {
+          C(i, j) = a + b;
+        }
+      }
+    }
+    return C;
+  }
+
   void Create::onData() {
     if (firstOnData) {
       if (model == CREATE_2) {
@@ -64,7 +91,7 @@ namespace create {
     // Get current time
     util::timestamp_t curTime = util::getTimestamp();
     float dt = (curTime - prevOnDataTime) / 1000000.0;
-    float deltaDist, deltaX, deltaY, deltaYaw;
+    float deltaDist, deltaX, deltaY, deltaYaw, leftWheelDist, rightWheelDist;
     if (model == CREATE_1) {
       /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
        * Angle returned is NOT correct if your robot is using older firmware:      *
@@ -75,6 +102,9 @@ namespace create {
       deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R
       deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) );
       deltaY = -deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
+      leftWheelDist = deltaDist / 2.0;
+      rightWheelDist = leftWheelDist;
+
     }
     else if (model == CREATE_2) {
       // Get cumulative ticks (wraps around at 65535)
@@ -95,9 +125,9 @@ namespace create {
       }
 
       // Compute distance travelled by each wheel
-      float leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
+      leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
                        * util::CREATE_2_WHEEL_DIAMETER * util::PI;
-      float rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
+      rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
                         * util::CREATE_2_WHEEL_DIAMETER * util::PI;
 
       float wheelDistDiff = rightWheelDist - leftWheelDist;
@@ -128,6 +158,69 @@ namespace create {
       vel.yaw = 0.0;
     }
 
+    // Update covariances
+    // Ref: "Introduction to Autonomous Mobile Robots" (Siegwart 2004, page 189)
+    float kr = 1.0; // TODO: Perform experiments to find these nondeterministic parameters
+    float kl = 1.0;
+    float cosYawAndHalfDelta = cos(pose.yaw + (deltaYaw / 2.0)); // deltaX?
+    float sinYawAndHalfDelta = sin(pose.yaw + (deltaYaw / 2.0)); // deltaY?
+    float distOverTwoWB = deltaDist / (util::CREATE_2_AXLE_LENGTH * 2.0);
+
+    Matrix invCovar(2, 2);
+    invCovar(0, 0) = kr * fabs(rightWheelDist);
+    invCovar(0, 1) = 0.0;
+    invCovar(1, 0) = 0.0;
+    invCovar(1, 1) = kl * fabs(leftWheelDist);
+
+    Matrix Finc(3, 2);
+    Finc(0, 0) = (cosYawAndHalfDelta / 2.0) - (distOverTwoWB * sinYawAndHalfDelta);
+    Finc(0, 1) = (cosYawAndHalfDelta / 2.0) + (distOverTwoWB * sinYawAndHalfDelta);
+    Finc(1, 0) = (sinYawAndHalfDelta / 2.0) + (distOverTwoWB * cosYawAndHalfDelta);
+    Finc(1, 1) = (sinYawAndHalfDelta / 2.0) - (distOverTwoWB * cosYawAndHalfDelta);
+    Finc(2, 0) = (1.0 / util::CREATE_2_AXLE_LENGTH);
+    Finc(2, 1) = (-1.0 / util::CREATE_2_AXLE_LENGTH);
+    Matrix FincT = boost::numeric::ublas::trans(Finc);
+
+    Matrix Fp(3, 3);
+    Fp(0, 0) = 1.0;
+    Fp(0, 1) = 0.0;
+    Fp(0, 2) = (-deltaDist) * sinYawAndHalfDelta;
+    Fp(1, 0) = 0.0;
+    Fp(1, 1) = 1.0;
+    Fp(1, 2) = deltaDist * cosYawAndHalfDelta;
+    Fp(2, 0) = 0.0;
+    Fp(2, 1) = 0.0;
+    Fp(2, 2) = 1.0;
+    Matrix FpT = boost::numeric::ublas::trans(Fp);
+
+    Matrix velCovar = ublas::prod(invCovar, FincT);
+    velCovar = ublas::prod(Finc, velCovar);
+
+    vel.covariance[0] = velCovar(0, 0);
+    vel.covariance[1] = velCovar(0, 1);
+    vel.covariance[2] = velCovar(0, 2);
+    vel.covariance[3] = velCovar(1, 0);
+    vel.covariance[4] = velCovar(1, 1);
+    vel.covariance[5] = velCovar(1, 2);
+    vel.covariance[6] = velCovar(2, 0);
+    vel.covariance[7] = velCovar(2, 1);
+    vel.covariance[8] = velCovar(2, 2);
+
+    Matrix poseCovarTmp = ublas::prod(poseCovar, FpT);
+    poseCovarTmp = ublas::prod(Fp, poseCovarTmp);
+    poseCovar = addMatrices(poseCovarTmp, velCovar);
+
+    pose.covariance[0] = poseCovar(0, 0);
+    pose.covariance[1] = poseCovar(0, 1);
+    pose.covariance[2] = poseCovar(0, 2);
+    pose.covariance[3] = poseCovar(1, 0);
+    pose.covariance[4] = poseCovar(1, 1);
+    pose.covariance[5] = poseCovar(1, 2);
+    pose.covariance[6] = poseCovar(2, 0);
+    pose.covariance[7] = poseCovar(2, 1);
+    pose.covariance[8] = poseCovar(2, 2);
+
+    // Update pose
     pose.x += deltaX;
     pose.y += deltaY;
     pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
@@ -171,7 +264,24 @@ namespace create {
   //}
 
   bool Create::setMode(const CreateMode& mode) {
-    return serial->sendOpcode((Opcode) mode);
+    switch (mode) {
+      case MODE_OFF:
+        return serial->sendOpcode(OC_POWER);
+        break;
+      case MODE_PASSIVE:
+        return serial->sendOpcode(OC_START);
+        break;
+      case MODE_SAFE:
+        return serial->sendOpcode(OC_SAFE);
+        break;
+      case MODE_FULL:
+        return serial->sendOpcode(OC_FULL);
+        break;
+      default:
+        CERR("[create::Create] ", "cannot set robot to mode '" << mode << "'");
+        break;
+    }
+    return false;
   }
 
   bool Create::clean(const CleanMode& mode) {