diff --git a/examples/odom_example.cpp b/examples/odom_example.cpp index 38dbc06..c45c5c7 100644 --- a/examples/odom_example.cpp +++ b/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 } diff --git a/include/create/create.h b/include/create/create.h index c9c35ea..03ef4cc 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -33,6 +33,7 @@ POSSIBILITY OF SUCH DAMAGE. #define CREATE_H #include +#include #include #include @@ -44,6 +45,8 @@ POSSIBILITY OF SUCH DAMAGE. namespace create { class Create { private: + typedef boost::numeric::ublas::matrix 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 data; diff --git a/include/create/types.h b/include/create/types.h index 9e65c08..6c3601d 100644 --- a/include/create/types.h +++ b/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; diff --git a/include/create/util.h b/include/create/util.h index e91d959..62e2bbb 100644 --- a/include/create/util.h +++ b/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::max() - std::abs(a) ); + } } // namespace util } // namespace create diff --git a/src/create.cpp b/src/create.cpp index 1fed6d0..3f8aa06 100644 --- a/src/create.cpp +++ b/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(new Data(model)); serial = boost::make_shared(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::min() : std::numeric_limits::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,10 +125,10 @@ namespace create { } // Compute distance travelled by each wheel - float leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV) - * util::CREATE_2_WHEEL_DIAMETER * util::PI; - float rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV) - * util::CREATE_2_WHEEL_DIAMETER * util::PI; + leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV) + * util::CREATE_2_WHEEL_DIAMETER * util::PI; + rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV) + * util::CREATE_2_WHEEL_DIAMETER * util::PI; float wheelDistDiff = rightWheelDist - leftWheelDist; deltaDist = (rightWheelDist + leftWheelDist) / 2.0; @@ -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) {