64 lines
2.2 KiB
C++
64 lines
2.2 KiB
C++
#include "create/create.h"
|
|
|
|
create::Create* robot;
|
|
|
|
int main(int argc, char** argv) {
|
|
std::string port = "/dev/ttyUSB0";
|
|
int baud = 115200;
|
|
create::RobotModel model = create::RobotModel::CREATE_2;
|
|
|
|
if (argc > 1 && std::string(argv[1]) == "create1") {
|
|
model = create::RobotModel::CREATE_1;
|
|
baud = 57600;
|
|
std::cout << "1st generation Create selected" << std::endl;
|
|
}
|
|
|
|
robot = new create::Create(model);
|
|
|
|
// Attempt to connect to Create
|
|
if (robot->connect(port, baud))
|
|
std::cout << "Successfully connected to Create" << std::endl;
|
|
else {
|
|
std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
robot->setMode(create::MODE_FULL);
|
|
|
|
usleep(1000000);
|
|
|
|
// 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 << 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
|
|
}
|
|
|
|
std::cout << "Stopping Create." << std::endl;
|
|
|
|
robot->disconnect();
|
|
delete robot;
|
|
|
|
return 0;
|
|
}
|