//* Direct Random Walk test *

#include "../00_nav_library/nav_aria_iface.h" #include "../00_nav_library/nav_random.h" #include <iostream> #include <iomanip> #include <conio.h> using namespace std; /* parameters to control driving functionality */ const long int drive_timeStep = 50; // Minimum time step used for driving computations const int drive_driveSpeed = 200; // linear speed when mapping const int drive_rotateSpeed = 20; // rotational speed when turning void waitUntilDone(NAV_Robot &R) { ArTime start; start.setToNow(); R.lock(); while( (R.isTryingToMove() && !R.isMoveDone()) || (!R.isHeadingDone()) ){ if (start.mSecSince()>5000) { std::cerr << "WARN: Action timed out." << std::endl; break; } R.unlock(); Sleep(50); R.lock(); } R.unlock(); } /* drive_RandomWallToWall for max_time ms or forever if 0 */ void drive_RandomWallToWall(NAV_Robot & R, long max_time) { #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif ArTime time; time.setToNow(); R.lock(); // we will keep robot locked while testing its sensors and issuing commands // but wil unlock it for significant amount of time while waiting or during lengthy computations while(max_time==0 || time.mSecSince()<max_time) { if ( !R.is_connected() ) { R.unlock(); // unlock before exiting the function return; } if ( R.isStalled() ) { R.Stop(); R.unlock(); // unlock before exiting the function return; } if ( R.getDistanceAhead() < R.getRobotLengthFront()+300. || R.getSonarDistance(-100, 100) < R.getRobotRadius()+100. ) { R.doDriveSpeed(0); if ( R.getSonarDistance(-100, -10) < R.getSonarDistance(10, 100) ) R.doRotateSpeed( getRandomInt(drive_rotateSpeed/2, drive_rotateSpeed) ); else R.doRotateSpeed( getRandomInt( -drive_rotateSpeed, -drive_rotateSpeed/2) ); while ( R.getDistanceAhead() < R.getRobotLengthFront()+300. || R.getSonarDistance(-100, 100) < R.getRobotRadius()+100. ) { R.unlock(); R.Sleep(drive_timeStep); // <-- this sleep must be executed while robot is unlocked R.lock(); } } R.doRotateSpeed(0); R.doDriveSpeed(drive_driveSpeed); R.unlock(); R.Sleep(drive_timeStep); // <-- this sleep must be executed while robot is unlocked R.lock(); #ifdef _DEBUG cerr <<"RNDW "<<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(6)<< R.getTh() << endl; #endif if (_kbhit()) /* <conio.h> */ { char c=tolower(_getch()); if (c=='q') break; } } R.Stop(); R.unlock(); } /* drive_RandomBrown for max_time ms or forever if 0 returns: 2+ reserved for future further exit conditions 1 robot blocked but not stalled - needs fine navigation 0 time lapsed -1 robot disconnected -2 robot stalled robot: stopped */ int drive_RandomBrown(NAV_Robot & R, long max_time) { #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif ArTime time; time.setToNow(); R.lock(); // we will keep robot locked while testing its sensors and issuing commands // but wil unlock it for significant amount of time while waiting or during lengthy computations while(max_time==0 || time.mSecSince()<max_time) { if ( !R.is_connected() ) { R.unlock(); // unlock before exiting the function return(-1); } if ( R.isStalled() ) { R.Stop(); R.unlock(); // unlock before exiting the function return(-2); } R.doRotateAngle( getRandomInt(-200, 200) ) ; R.unlock(); waitUntilDone(R); // waitUntilDone() has a loop inside and locks robot to test if a command has been completed // and unlocks the robot to sleep while waiting until it is completed R.lock(); #ifdef _DEBUG cerr <<"RNDB "<<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(6)<< R.getTh() << endl; #endif R.doDriveDistance( getRandomInt(0, R.getDistanceAhead()-R.getRobotLengthFront() ) ); R.unlock(); waitUntilDone(R); // waitUntilDone() has a loop inside and locks robot to test if a command has been completed // and unlocks the robot to sleep while waiting until it is completed R.lock(); #ifdef _DEBUG cerr <<"RNDB "<<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(6)<< R.getTh() << endl; #endif if (_kbhit()) /* <conio.h> */ { char c=tolower(_getch()); if (c=='q') break; } } R.Stop(); R.unlock(); return(0); } 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); } drive_RandomWallToWall(R, 0); // drive_RandomBrown(R, 0); if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; return(0); }