//* Mapping using Direct Random Walk test *

#include "../00_nav_library/nav_aria_iface.h" #include "../00_nav_library/nav_random.h" #include "../00_nav_library/nav_map.h" #include "../00_nav_library/nav_robot2map.h" #include "../00_nav_library/nav_geometry.h" #include <iostream> #include <iomanip> #include <conio.h> using namespace std; const long int map_timeSavePartial = 180000L; // Maximum time between saving map backups const long int drive_timeStep = 50; // Minimum time step used for several things const int drive_driveSpeed = 200; // linear speed when mapping const int drive_rotateSpeed = 20; // rotational speed when turning /* drive_RandomWallToWall for max_time ms or forever if 0 */ void drive_RandomWallToWallMapping(NAV_Robot & R, long max_time, NAV_Map &MAP) { #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif // this code portion is for testing returning to the preset position only (has nothing to do with mapping) R.lock(); NAV_Proximity POINT( R.getPose(), R.getRobotRadius()*5 ); R.unlock(); // this code portion is for testing crossing a preset line only (has nothing to do with mapping) R.lock(); //NAV_LineCrossing SIDES( R.getX(), R.getY(), R.getX() + 1*ArMath::cos(R.getTh()), R.getY() + 1*ArMath::sin(R.getTh()) ); NAV_LineCrossing SIDES( R.getX(), R.getY(), R.getTh() ); R.unlock(); ArTime maptime; maptime.setToNow(); ArTime runtime; runtime.setToNow(); R.lock(); while(max_time==0 || runtime.mSecSince()<max_time) { if ( !R.is_connected() ) { R.unlock(); return; } if ( R.isStalled() ) { R.Stop(); R.unlock(); return; } // this code portion is for testing returning to the preset position only (has nothing to do with mapping) if (POINT.checkReturned(R.getPose())) { POINT.resetReturned(); cout << "*** returned to remembered position ***" << endl; } // this code portion is for testing crossing a preset line only (has nothing to do with mapping) if (SIDES.checkCrossed(R.getPose())) { cout << "*** crossed between left and right half plains ***" << endl; } if ( R.getDistanceAhead() < R.getRobotLengthFront()+300. || R.getSonarDistance(-100, 100) < R.getRobotRadius()+200. ) { 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) ); R.unlock(); R.Sleep(drive_timeStep); R.lock(); while ( R.getDistanceAhead() < R.getRobotLengthFront()+300. || R.getSonarDistance(-100, 100) < R.getRobotRadius()+200. ) { R.unlock(); map_RobotBuffer(R,MAP); // example 2 is inside R.Sleep(drive_timeStep); R.lock(); } R.doRotateSpeed(0) ; } R.doDriveSpeed(drive_driveSpeed); R.unlock(); R.Sleep(drive_timeStep); R.lock(); R.unlock(); map_RobotBuffer(R,MAP); // example 2 is inside 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; else if (c=='s') { R.Stop(); R.unlock(); // unlocking robot before a time consuming task cout << "Paused for saving the map file. . ."; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); maptime.setToNow(); cout << " Done!" << endl; R.lock(); } else if (c=='p') { // this code portion is for testing returning to the preset position only (has nothing to do with mapping) POINT.setPose(R.getPose()); // this code portion is for testing crossing a preset line only (has nothing to do with mapping) SIDES = NAV_LineCrossing( R.getX(), R.getY(), R.getTh() ); cout << "*** position and line remembered ***" << endl; } } if (maptime.mSecSince()>=map_timeSavePartial) { R.Stop(); R.unlock(); // unlocking robot before a time consuming task cout << "Paused for saving the map file. . ."; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); maptime.setToNow(); cout << " Done!" << endl; R.lock(); } } R.Stop(); R.unlock(); } 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); } NAV_Map MAP(100); R.moveCoords(ArPose(0,0,0)); cout << "Assuming the same starting position as at the previous run,\nwould you like to load an existing map? (y/N) "; char ans=tolower(_getch()); if (ans=='y') { cout << endl << "Reading. . . "; if (MAP.read("map.ppm")) cout << "Last map read as reqested." << endl; else cout << "Reading of the last map failed, continuing with a new map." << endl; } else { cout << endl << "Starting with a new map." << endl; }

//* Mapping using sudden movement changes is actually a bad idea *

drive_RandomWallToWallMapping(R, 0, MAP); if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; cout << "Saving the map file. . ." << endl; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); cout << "Done!" << endl; return(0); }