//* Running in circles action test *

#include "../00_nav_library/nav_aria_iface.h" #include <iostream> #include <iomanip> #include <conio.h> using namespace std; int main(){ // NAV_Robot R("gdansk.bradley.edu", 8101); NAV_Robot R("localhost", 8101); // NAV_Robot R("com1"); // NAV_Robot R("com4"); if (!R.is_connected()) { cout << "Cannot connect to the robot or robot simulator, exiting" << endl; return(0); } // Collision avoidance actions at higher priority ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250); ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400); ArActionLimiterTableSensor tableLimiterAction; R.lock(); R.addAction(&tableLimiterAction, 100); R.addAction(&limiterAction, 95); R.addAction(&limiterFarAction, 90); R.unlock(); // Goto action at lower priority ArActionGoto gotoPoseAction("goto"); R.lock(); R.addAction(&gotoPoseAction, 30); R.unlock(); // Stop action at lower priority, so the robot stops if it has no goal ArActionStop stopAction("stop"); R.lock(); R.addAction(&stopAction, 20); R.unlock(); const long int duration = 120000; //sec - timeout bool first = true; int goalNum = 0; ArTime start; start.setToNow(); #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif while( R.is_connected()) { // keyboard exit if (_kbhit()) /* <conio.h> */ { char c=tolower(_getch()); if (c=='q') break; } R.lock(); // Choose a new goal if this is the first loop iteration, or if we // achieved the previous goal. if (first || gotoPoseAction.haveAchievedGoal()) { first = false; goalNum++; if (goalNum > 4) goalNum = 1; // start again at goal #1 // set our positions for the different goals if (goalNum == 1) gotoPoseAction.setGoal(ArPose(2500, 0)); else if (goalNum == 2) gotoPoseAction.setGoal(ArPose(2500, 2500)); else if (goalNum == 3) gotoPoseAction.setGoal(ArPose(0, 2500)); else if (goalNum == 4) gotoPoseAction.setGoal(ArPose(0, 0)); #ifdef _DEBUG cerr <<"RNDW new goal: " << gotoPoseAction.getGoal().getX() << " , " << gotoPoseAction.getGoal().getY() << endl; #endif } if( start.mSecSince()>=duration ) { gotoPoseAction.cancelGoal(); R.unlock(); #ifdef _DEBUG cerr <<"RNDW timeout - goal not reached within specified time frame" << endl; #endif break; } R.unlock(); #ifdef _DEBUG cerr <<"RNDW "<<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(6)<< R.getTh() << endl; #endif R.Sleep(100); } R.lock(); R.Stop(); R.unlock(); R.lock(); if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; R.unlock(); return(0); }