Add covariance info to Pose and Vel
Fix getMode bug
This commit is contained in:
Jacob Perron
parent
1133a0e4bb
commit
82b01e4057
5 changed files with 149 additions and 13 deletions
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
122
src/create.cpp
122
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,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) {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue