int main (int argc, char** argv) {
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
//ArAnalogGyro gyro = new
ArAnalogGyro gyro(&robot);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given, just exit.
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
ArSonarDevice sonarDev;
// robot.runAsync(true);
// ArMap map("office.map");
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
// map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
// map.setIgnoreCase(true);
robot.runAsync(true);
robot.enableMotors();
robot.moveTo(ArPose(0,0,0));
//robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.addRangeDevice(&sonarDev);
//robot.unlock();
// ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
<<<<<<< HEAD
int main(int argc, char **argv)
{
//----------------------initialized robot server------------------------------------------
Aria::init();
Arnl::init();
ArServerBase server;
//-----------------------------------------------------------------------------------
VCCHandler ptz(&robot); //create keyboard for control vcc50i
G_PTZHandler->reset();
ArUtil::sleep(300);
G_PTZHandler->panSlew(30);
//-----------------------------------------------------------------------------------
argc = 2 ;
argv[0] = "-map";
argv[1] = "map20121111.map";
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Set up our simpleConnector
ArSimpleConnector simpleConnector(&parser);
// Set up our simpleOpener
ArServerSimpleOpener simpleOpener(&parser);
//*******
// Set up our client for the central server
// ArClientSwitchManager clientSwitch(&server, &parser);
//************
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
// parser.loadDefaultArguments();
// set up a gyro
ArAnalogGyro gyro(&robot);
//gyro.activate();
// Parse arguments for the simple connector.
// if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
// {
// ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
// Aria::logOptions();
// Aria::exit(1);
// }
// The laser object, will be used if we have one
// Add the laser to the robot
robot.addRangeDevice(&sick);
// Sonar, must be added to the robot, used by teleoperation and wander to
// detect obstacles, and for localization if SONARNL
ArSonarDevice sonarDev;
// Add the sonar to the robot
robot.addRangeDevice(&sonarDev);
// Set up where we'll look for files
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps");
ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
// Set up the map, this will look for files in the examples
// directory (unless the file name starts with a /, \, or .
// You can take out the 'fileDir' argument to look in the current directory
// instead
ArMap arMap(fileDir);
// set it up to ignore empty file names (otherwise the parseFile
// on the config will fail)
arMap.setIgnoreEmptyFileName(true);
//********************************
//Localization
//********************************
ArLocalizationManager locManager(&robot, &arMap);
ArLog::log(ArLog::Normal, "Creating laser localization task");
ArLocalizationTask locTask (&robot, &sick, &arMap);
locManager.addLocalizationTask(&locTask);
//*******************************
//Path planning
//*******************************
// Make the path task planning task
ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
G_PathPlanning = &pathTask;
// Set up things so data can be logged (only do it with the laser
// since it can overrun a 9600 serial connection which the sonar is
// more likely to have)
ArDataLogger dataLogger(&robot);
dataLogger.addToConfig(Aria::getConfig());
// add our logging to the config
// ArLog::addToConfig(Aria::getConfig());
// First open the server
if (!simpleOpener.open(&server, fileDir, 240))
{
if (simpleOpener.wasUserFileBad())
ArLog::log(ArLog::Normal, "Bad user file");
else
ArLog::log(ArLog::Normal, "Could not open server port");
exit(2);
}
// Connect the robot
if (!simpleConnector.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
Aria::exit(3);
}
//-----------------------------------------------
//**************************
// Set up a class that'll put the movement and gyro parameters into ArConfig
ArRobotConfig robotConfig(&robot);
robotConfig.addAnalogGyro(&gyro);
//*****************************
robot.enableMotors();
robot.clearDirectMotion();
// if we are connected to a simulator, reset it to its start position
robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);
robot.moveTo(ArPose(0,0,0));
// Set up laser using connector (command line arguments, etc.)
simpleConnector.setupLaser(&sick);
// Start the robot thread.
robot.runAsync(true);
// Start the laser thread.
sick.runAsync();
// Try to connect the laser
if (!sick.blockingConnect())
ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used");
else
ArLog::log(ArLog::Normal, "Connected to laser.");
//***************************************
// Add additional range devices to the robot and path planning task.
// IRs if the robot has them.
robot.lock();
// ArIRs irs;
// robot.addRangeDevice(&irs);
// pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
//******************************************
// Forbidden regions from the map
ArForbiddenRangeDevice forbidden(&arMap);
robot.addRangeDevice(&forbidden);
// This is the place to add a range device which will hold sensor data
// and delete it appropriately to replan around blocked paths.
ArGlobalReplanningRangeDevice replanDev(&pathTask);
// Create objects that add network services:
// Drawing in the map display:
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
drawings.addRangeDevice(&replanDev);
/* If you want to draw the destination put this code back in:
ArServerDrawingDestination destination(
&drawings, &pathTask, "destination",
500, 500,
new ArDrawingData("polyDots",
ArColor(0xff, 0xff, 0x0),
800, // size
49), // just below the robot
*/
/* If you want to see the local path planning area use this
(You can enable this particular drawing from custom commands
which is set up down below in ArServerInfoPath)
ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP(pathTask, &ArPathPlanningTask::drawSearchRectangle);
drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP);
*/
/* If you want to see the points making up the local path in addition to the
* main path use this.
ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints);
drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2);
*/
// Misc. simple commands:
ArServerHandlerCommands commands(&server);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
serverInfoPath.addSearchRectangleDrawing(&drawings);
serverInfoPath.addControlCommands(&commands);
//-------------------------receive commands or events from client---------------------
// ArServerHandlerCommands commands(&server);
server.addData("RobotVideo" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&VideoServerBase::RobotVideoCB) ,"","");
server.addData("turn" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&turn_func) ,"","");
server.addData("RobotMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotMotion) ,"","");
server.addData("CameraMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_CameraMotion) ,"","");
server.addData("RobotTurnLeft" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnLeft) ,"","");
server.addData("RobotTurnRight" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnRight) ,"","");
server.addData("TargetApproach" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach) ,"","");
server.addData("TargetApproachObstacles" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach_Obstacles) ,"","");
server.addData("GlassesCancel" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_GlassesCancel) ,"","");
server.addData("Calibration" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_Calibration) ,"","");
server.addClientRemovedCallback(new ArGlobalFunctor1< ArServerClient * >(&clientCloseCallback));
//-------------------------receive commands or events from client---------------------
//***********************
ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager);
ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager);
// Provide the map to the client (and related controls):
// This uses both lines and points now, since everything except
// sonar localization uses both (path planning with sonar still uses both)
ArServerHandlerMap serverMap(&server, &arMap);
// Add some simple (custom) commands for testing and debugging:
ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // monitor the gyro
ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters
ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.)
/* Set up the possible modes for remote control from a client such as
* MobileEyes:
*/
// Mode To go to a goal or other specific point:
ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap, ArPose(0,0,0));
// Add a simple (custom) command that allows you to give a list of
// goals to tour, instead of all. Useful for testing and debugging.
modeGoto.addTourGoalsInListSimpleCommand(&commands);
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&server, &robot);
// cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, this should not be done if you need the sonar
// data to localize, or for other purposes while stopped)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Teleoperation modes To drive by keyboard, joystick, etc:
ArServerModeRatioDrive modeRatioDrive(&server, &robot); // New, improved mode
ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability
// Drive mode's configuration and custom (simple) commands:
modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
modeDrive.addControlCommands(&commands);
modeRatioDrive.addControlCommands(&commands);
// Wander mode
// ArServerModeWander modeWander(&server, &robot);
//*********************************
// Prevent driving if localization is lost:
ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive);
modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110);
// Prevent wandering if lost:
// ArActionLost
// actionLostWander (&locManager, NULL, &modeWander);
// modeWander.getActionGroup ()->addAction (&actionLostWander, 110);
// This provides a small table of interesting information for the client
// to display to the operator:
ArServerInfoStrings stringInfo(&server);
Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(&robot,
&ArRobot::getMotorPacCount));
// Make Stop mode the default (If current mode deactivates without entering
// a new mode, then Stop Mode will be selected)
modeStop.addAsDefaultMode();
// Create the service that allows the client to monitor the communication
// between the robot and the client.
//
ArServerHandlerCommMonitor handlerCommMonitor(&server);
// Create service that allows client to change configuration parameters in ArConfig
ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
// Read in parameter files. read the paras from input
Aria::getConfig()->useArgumentParser(&parser);
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
// Warn about unknown params.
if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
simpleConnector.logOptions();
simpleOpener.logOptions();
Aria::exit(6);
}
// Warn if there is no map
if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
{
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal, "### Warning, No map file is set up, you can make a map with sickLogger or arnlServer, and Mapper3; More info in docs/Mapping.txt and README.txt. Set the map with the -map command line option, or by changing the config with MobileEyes or by editing the config file.");
ArLog::log(ArLog::Normal, "");
}
// find out where we'll want to put files
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal,
"Directory for maps and file serving: %s", fileDir);
ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
ArLog::log(ArLog::Normal, "");
// If you want MobileSim to try and load up the same map as you are
// using in guiServer then uncomment out the next line and this object
// will send a command to MobileSim to do so, but make sure you start
// MobileSim from the Arnl/examples directory or use the --cwd option,
// so that the map names used by MobileSim match the map names used
// by guiServer
//ArSimMapSwitcher mapSwitcher(&robot, &arMap);
/******************************************************
* ****************************************************
* Camera
* ****************************************************
* *****************************************************/
robot.unlock();
// Localize robot at home.
locTask.localizeRobotAtHomeBlocking();
locTask.forceUpdatePose(ArPose(0,0,0));
resetMotion();
// robot.enableMotors();
// robot.runAsync(true);
//locTask.localizeRobotAtHomeBlocking();
server.runAsync();
VideoServerBase videoserver;
videoserver.runAsync();
robot.comInt(ArCommands::SOUNDTOG, 0);
//G_PathPlanning->pathPlanToPose(ArPose(1500, -1500 , -32),true,true);
//while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );
//S_TargetApproach_Obstacles1();
//ArUtil::sleep(5000);
// G_PTZHandler->panRel(60);
// ArUtil::sleep(3000);
//while(1)
//S_TargetApproach1();
cout << "done!!!!!!!---------------------" <<endl;
robot.waitForRunExit();
Aria::exit(0);
}
int main (int argc, char** argv) {
Aria::init();
ArRobot robot;
ArSonarDevice sonar;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
ArSonarDevice sonarDev;
ArPose* poseList = readPostitions("positions.txt");
robot.runAsync(true);
robot.enableMotors();
robot.moveTo(ArPose(0,0,0));
robot.comInt(ArCommands::ENABLE, 1);
robot.addRangeDevice(&sonarDev);
ArActionGoto gotoPoseAction("goto", ArPose(0, 0, 0), 200);
ArActionAvoidFront avoidFront("avoid front");
ArActionStallRecover stallRecover("stall recover");
robot.addAction(&gotoPoseAction, 50);
robot.addAction(&avoidFront, 60);
robot.moveTo(ArPose(0,0,0));
int length = ARRAY_SIZE(poseList);
cout<<"do dai"<<length;
ArServerBase server;
ArServerSimpleOpener simpleOpener(&parser);
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
"ArNetworking/examples");
// first open the server up
if (!simpleOpener.open(&server, fileDir, 240))
{
if (simpleOpener.wasUserFileBad())
printf("Bad user/password/permissions file\n");
else
printf("Could not open server port\n");
exit(1);
}
ArServerInfoRobot serverInfo(&server, &robot);
GotoGoal gotoGoal(&robot, &sonar, &server, &serverInfo);
gotoGoal.init(argc, argv);
float angle = 0;
VideoCapture cap;
cap.open(0);
Rect trackWindow;
//var check find ball
bool checkObject = false;
int hsize = 16;
namedWindow( "threshold", 0 );
namedWindow( "trackbar", 0 );
namedWindow( "Histogram", 0 );
namedWindow( "main", 0 );
createTrackbar( "Vmin", "trackbar", &vmin, 256, 0 );
createTrackbar( "Vmax", "trackbar", &vmax, 256, 0 );
createTrackbar( "Smin", "trackbar", &smin, 256, 0 );
CascadeClassifier c;
c.load("cascade.xml");
Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
float vel = 0;
int i = 0;
while(1)
{
cap >> frame;
if( frame.empty() ){
cout<<"error camera"<<endl;
break;
}
frame.copyTo(image);
cvtColor(image, hsv, COLOR_BGR2HSV);
int _vmin = vmin, _vmax = vmax;
inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)), Scalar(180, 256, MAX(_vmin, _vmax)), mask);
gotoPoseAction.setGoal(poseList[i]);
while (!gotoPoseAction.haveAchievedGoal())
{
ArLog::log(ArLog::Normal, "goal(%.2f, %0.2f) x = %.2f, y = %.2f", poseList[i].getX(), poseList[i].getY(), robot.getX(), robot.getY());
// if (!checkObject)
checkObject = detect(frame, c);
if (checkObject)
cout <<"Phat hien doi tuong"<<endl;
else
cout <<"Khong phat hien doi tuong"<<endl;
if (checkObject) {
if(trackObject(hsv, mask)) {
float d = distance();
if (d < 250) {
gotoGoal.move(-200);
} else if ( d >= 250 && d <= 300) {
gotoGoal.stop();
}
else {
vel = d * 0.7;
vel = (int) (vel/50) * 50;
if(vel > 200) {
vel = 200;
gotoGoal.setVel(vel);
}
angle = determindRotate();
cout <<"khoang cach: "<<d<<"\tGoc quay: "<<angle<<"\t van toc = "<<vel<<endl;
if (angle != 0) {
gotoGoal.stop();
gotoGoal.rotate(angle);
}
}
}
}
imshow("main", image);
imshow( "threshold", mask );
imshow( "Histogram", histimg );
}
i++;
}
ArUtil::sleep(2000);
Aria::shutdown();
}
int main (int argc, char** argv) {
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given, just exit.
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
ArSonarDevice sonarDev;
ArLog::log(ArLog::Normal, "OK");
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "examples");
ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
// strcpy(fileDir, "columbia.map");
ArLog::log(ArLog::Normal, "file Maps directory is: %s\n",fileDir);
ArMap map(fileDir);
map.readFile("columbia.map");
// set it up to ignore empty file names (otherwise the parseFile
// on the config will fail)
map.setIgnoreEmptyFileName(true);
robot.addRangeDevice(&sonarDev);
// set home pose
robot.moveTo(ArPose(0,0,0));
ArPathPlanningTask pathPlan(&robot, &sonarDev, &map);
ArActionGoto gotoPoseAction("goto");
// gotoPoseAction.activate();
// pathPlan.getPathPlanActionGroup()->addAction(&gotoPoseAction, 50);
// pathPlan.getPathPlanAction()->activate();
ArForbiddenRangeDevice forbidden(&map);
robot.addRangeDevice(&forbidden);
pathPlan.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
// pathPlan.planAndSetupAction(ArPose(0, 0, 0));
// pathPlan.
/*
if (pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true))
ArLog::log(ArLog::Normal, "OK");
else
ArLog::log(ArLog::Normal, "FAILED");
*/
// create functor
ArGlobalFunctor1<ArPose> goalDone(&addGoalDone);// = new ArGlobalFunctor1(&addGoalDone);
ArGlobalFunctor1<ArPose> goalFail(&addGoalFailed);
ArGlobalFunctor1<ArPose> goalFinished(&addGoalFinished);
ArGlobalFunctor1<ArPose> goalInterrupted(&addGoalInterrupted);
// add functor
pathPlan.addGoalDoneCB(&goalDone);
pathPlan.addGoalFailedCB(&goalFail);
pathPlan.addGoalFinishedCB(&goalFinished);
pathPlan.addGoalInterruptedCB(&goalInterrupted);
robot.runAsync(true);
robot.enableMotors();
pathPlan.runAsync();
// ArPathPlanningTask::PathPlanningState state = pathPlan.getState();
// while(!pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true);
ArPathPlanningTask::PathPlanningState state = pathPlan.getState();
char* s = "";
switch(state)
{
case ArPathPlanningTask::NOT_INITIALIZED: {s = "NOT_INITIALIZED"; break;}
case ArPathPlanningTask::PLANNING_PATH: { s = "PLANNING_PATH";break;}
case ArPathPlanningTask::MOVING_TO_GOAL:{s = "MOVING_TO_GOAL";break;}
case ArPathPlanningTask::REACHED_GOAL: {s = "REACHED_GOAL";break;}
case ArPathPlanningTask::FAILED_PLAN: { s = "FAILED_PLAN";break;}
case ArPathPlanningTask::FAILED_MOVE: { s="FAILED_MOVE";break;}
case ArPathPlanningTask::ABORTED_PATHPLAN:{s = "ABORTED_PATHPLAN";break;}
// case ArPathPlanningTask::INVALID:
// default:
// return "UNKNOWN";
}
ArLog::log(ArLog::Normal,s);
robot.waitForRunExit();
// pathPlan.
// char* text = new char[512];
// pathPlan.getStatusString(text, sizeof(text));
// printf("Planning status: %s.\n", text);
// while(pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
Aria::shutdown();
// Arnl::s;
Aria::exit(0);
}
void Joydrive::drive(void)
{
int trans, rot;
ArPose pose;
ArPose rpose;
ArTransform transform;
ArRangeDevice *dev;
ArSensorReading *son;
if (!myRobot->isConnected())
{
printf("Lost connection to the robot, exiting\n");
exit(0);
}
printf("\rx %6.1f y %6.1f th %6.1f",
myRobot->getX(), myRobot->getY(), myRobot->getTh());
fflush(stdout);
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
if (ArMath::fabs(myRobot->getVel()) < 10.0)
myRobot->comInt(ArCommands::ENABLE, 1);
myJoyHandler.getAdjusted(&rot, &trans);
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
time(NULL) - myLastPress > 1)
{
myLastPress = time(NULL);
printf("\n");
switch (myTest)
{
case 1:
printf("Moving back to the origin.\n");
pose.setPose(0, 0, 0);
myRobot->moveTo(pose);
break;
case 2:
printf("Moving over a meter.\n");
pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
myRobot->moveTo(pose);
break;
case 3:
printf("Doing a transform test....\n");
printf("\nOrigin should be transformed to the robots coords.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(0, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
printf("\nRobot coords should be transformed to the origin.\n");
transform = myRobot->getToLocalTransform();
pose = myRobot->getPose();
pose = transform.doTransform(pose);
rpose.setPose(0, 0, 0);
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
break;
case 4:
printf("Doing a tranform test...\n");
printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(-1000, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
printf("Probable Success\n");
else
printf("#### FAILURE\n");
break;
case 5:
printf("Doing a transform test on range devices..\n");
printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
dev = myRobot->findRangeDevice("sonar");
if (dev == NULL)
{
printf("No sonar on the robot, can't do the test.\n");
break;
}
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
pose = myRobot->getPose();
pose.setX(pose.getX() + 4000);
pose.setY(pose.getY() + 4000);
myRobot->moveTo(pose);
printf("Moved robot.\n");
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
break;
case 6:
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
printf("Disconnecting from the robot, then reconnecting.\n");
myRobot->disconnect();
myRobot->blockingConnect();
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
break;
default:
printf("No test for second button.\n");
break;
}
}
}
int main( int argc, char **argv ){
// parse our args and make sure they were all accounted for
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
ArSick sick;
double dist, angle = 0;
// Allow for esc to release robot
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
if( !connector.parseArgs() || argc > 1 ){
connector.logOptions();
exit(1);
}
// add the laser to the robot
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if( !connector.connectRobot(&robot) ){
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
connector.setupLaser(&sick);
sick.runAsync();
if( !sick.blockingConnect() ){
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::ENABLE, 1);
ArPose pose(0, -1000, 0);
robot.moveTo(pose);
ArPose prev_pose = robot.getPose();
double total_distance = 0;
printf("Connected\n");
ArUtil::sleep(1000);
PathLog log("../Data/reactive.dat");
int iterations_wo_movement = 0;
while( iterations_wo_movement < CONSECUTIVE_NON_MOTIONS ){
// Get updated measurement
sick.lockDevice();
dist = sick.currentReadingPolar(-90, 90, &angle);
sick.unlockDevice();
dist = (dist > 30000) ? 0 : dist - IDEAL_DISTANCE;
trackRobot(&robot, dist, angle);
ArUtil::sleep(500);
pose = robot.getPose();
log.write(pose);
total_distance += getDistance(prev_pose, pose);
prev_pose = pose;
// Determine if the robot is done tracking
isRobotTracking(&iterations_wo_movement, dist, angle);
}
ArUtil::sleep(1000);
log.close();
ofstream output;
output.open("../Data/reactive_dist.dat", ios::out | ios::trunc);
output << "Reactive 1 " << total_distance << endl;
output.close();
Aria::exit(0);
return 0;
}
