compAIR/libcreate
compAIR/libcreate
4
0
Fork 1
Code Issues Pull requests Projects Releases Packages 1 Wiki Activity Actions

Add covariance info to Pose and Vel

Browse source 
Fix getMode bug
This commit is contained in:
Jacob Perron 2016-04-14 16:40:56 -07:00
parent 1133a0e4bb
commit 82b01e4057
5 changed files with 149 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

18
examples/odom_example.cpp
View file

@ -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
}