//* Mapping using Random Walk Action test *

#include "../00_nav_library/nav_aria_iface.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; 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); } // initialize the map and ask whether to load exisiting map NAV_Map MAP( 100, R.getRobotRadius(), 2500 ); // Note: R.getSonarDistanceTooFar() is not the range of robot sonars!!! R.lock(); R.moveCoords(ArPose(0,0,0)); R.unlock(); // this code portion is for testing returning to the preset postion 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(); 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; } // add a set of actions that combine together to effect the wander behavior ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0); ArActionAvoidFront avoidFrontFar; ArActionConstantVelocity constantVelocity("Constant Velocity", 400); // ArActionJoydrive works only if joystick present, ignored otherwise (has nothing to do with mapping) ArActionJoydrive joystick("joydrive", 400, 15, false, true); joystick.setThrottleParams(100, 400); // throttle used only if this is called if (joystick.joystickInited()) { cout << "JOYSTICK ovverride is on.\nPress a joystick button before starting the joystick control override." << endl; } R.lock(); R.addAction(&recover, 100); R.addAction(&bumpers, 75); R.addAction(&avoidFrontNear, 50); R.addAction(&avoidFrontFar, 49); R.addAction(&joystick, 40); R.addAction(&constantVelocity, 25); R.unlock(); #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif // robot mapping ArTime maptime; maptime.setToNow(); // example 1A R.SMlock(); const NAV_Robot::pListpArPWT ML = R.SMgetCumulativeBuffer(); // this is a pointer to list<ArPoseWithTime*> R.SMunlock(); ArTime runtime; runtime.setToNow(); while( R.is_connected()) { R.Sleep(50); // this code portion is for testing returning to the preset postion only (has nothing to do with mapping) R.lock(); if (POINT.checkReturned(R.getPose())) { POINT.resetReturned(); cout << "*** returned to remembered position ***" << endl; } R.unlock(); // this code portion is for testing crossing a preset line only (has nothing to do with mapping) R.lock(); if (SIDES.checkCrossed(R.getPose())) { cout << "*** crossed between left and right half plains ***" << endl; } R.unlock(); map_RobotBuffer(R,MAP); // example 2 is inside // example 1B R.SMlock(); // if forget to lock the exported data may be corrupt int recentpointcount = static_cast<int>(ML->size()); // accessing pointer to the list that had been saved at the very beginning R.SMunlock(); // if forget to unlock then robot cannot pump new reading into the sensor and all interface may halt map_RobotBuffer(R,MAP); // example 2 is inside #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.lock(); R.Stop(); // any direct motion command will take precedence over all actions R.unlock(); cout << "Paused for saving the map file. . ."; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); maptime.setToNow(); cout << " Done!" << endl; R.lock(); R.clearDirectMotion(); // this is the way to cancel stop() or doXXX(...) commands R.unlock(); } else if (c=='p') { // this code portion is for testing returning to the preset postion only (has nothing to do with mapping) R.lock(); POINT.setPose(R.getPose()); R.unlock(); // this code portion is for testing crossing a preset line only (has nothing to do with mapping) R.lock(); SIDES = NAV_LineCrossing( R.getX(), R.getY(), R.getTh() ); R.unlock(); cout << "*** position and line remembered ***" << endl; } } if (maptime.mSecSince()>=180000L) { R.lock(); R.Stop(); // any direct motion command will take precedence over all actions R.unlock(); cout << "Paused for saving the map file. . ."; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); maptime.setToNow(); cout << " Done!" << endl; R.lock(); R.clearDirectMotion(); // this is the way to cancel stop() or doXXX(...) commands R.unlock(); } } R.lock(); R.deactivateActions(); R.unlock(); if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; // saving the latest map cout << "Saving the map file. . ." << endl; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); cout << "Done!" << endl; return(0); }