//* Driving using both Potential Field and Flooding Algorithms *

// TO DO: Homework - refine the constant to prevent oscilations and ensure stability #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_field_potential.h" #include "../00_nav_library/nav_path_flood.h" #include <iostream> #include <iomanip> #include <conio.h> using namespace std; int drive_ToGoal(NAV_Robot & R, NAV_Map &MAP, NAV_FIELD_Potential &POT, NAV_Path_Flood &FLOOD) { #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif unsigned int MODE = 0; // 0 == use POT, 1 == use FLOOD while(true) { R.lock(); if ( !R.is_connected() ) { R.unlock(); return(-1); } if ( R.isStalled() ) { R.Stop(); R.unlock(); return(-2); } R.unlock(); // recommendation from POT (uses MAP updates on the fly but you can get stack in a local minimum) double Pvelocity = 0; double Pheading = 0; // recommendation from FLOOD (done quickly once path calculated but no MAP updates are used) double Fvelocity = 0; double Fheading = 0; // this is what we will do double velocity = 0; double heading = 0; R.lock(); ArPose position = R.getPose(); R.unlock(); if (POT.is_goal(position) || FLOOD.is_goal(position) ) { break; } Pvelocity = POT.getVelCoef(position); Pheading = POT.getHeading(position); Fvelocity = FLOOD.getVelCoef(position); Fheading = FLOOD.getHeading(position); if (MODE==0) { if (Pvelocity<0.05) { // Local Minimum or goal reached (that was already checked MODE=1; cout << "Switching to flooding. . ." << endl; } } else if (MODE==1) { if ( ArMath::fabs( ArMath::fixAngle(Fheading-Pheading) ) < 50 && Pvelocity>=0.05 ) { // FLOOD goes in the same direction that POT again and no local minimum, switch to POT MODE=0; cout << "Switching to pot field. . ." << endl; } } else { cerr << "ERROR Internal program error: illegal MODE value" << endl; break; } if (MODE==0) { // POT velocity = 200. * Pvelocity; heading = Pheading; } else if (MODE==1) { // FLOOD if ( !FLOOD.is_reachable(position)) { cerr << "WARN: destination unreachable!" << endl; break; // destination unreachable } velocity = 100. * Fvelocity; heading = Fheading; } else { cerr << "ERROR Internal program error: illegal MODE value" << endl; break; } R.lock(); R.doDriveSpeed(velocity); R.doRotateToAngle(heading); R.unlock(); #ifdef _DEBUG R.lock(); cerr <<"PATH " <<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(6)<< R.getTh() <<" V:"<<setw(4)<<velocity <<" H:"<<setw(4)<< heading <<" M="<<setw(1)<< MODE << endl; R.unlock(); #endif if (_kbhit()) /* <conio.h> */ { char c=tolower(_getch()); if (c=='q') break; else if (c=='s') { R.lock(); R.Stop(); R.unlock(); cout << "Paused for saving the map file. . ."; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); cout << " Done!" << endl; } } R.Sleep(20); // <-- this sleep must be executed while robot is unlocked map_RobotBuffer(R,MAP); } R.lock(); 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); } NAV_Map MAP(100); NAV_FIELD_Potential POT(&MAP); /* we use default values for the remaining paramteres */ NAV_Path_Flood FLOOD(&MAP); /* we use default values for the remaining paramteres */ 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; } for (;;) { double x, y, th; cout << "Please enter the next destination (X Y Th) in robot's coordinate system "; cin >> x >> y >> th; if (cin.fail()) { cin.clear(); char c; cin >> c; cin.ignore(256,'\n'); if (c=='q') break; else continue; } ArPose dest(x, y, th); POT.setGoal(dest); FLOOD.setGoal(dest);

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

drive_ToGoal(R, MAP, POT, FLOOD); if (POT.is_close(R.getPose()) || FLOOD.is_close(R.getPose()) ) { cout << "Goal reachd. The current position is " << R.getX() << " " << R.getY() << " " << R.getTh() << endl; } else { cout << "Goal FAILED. The current position is " << R.getX() << " " << R.getY() << " " << R.getTh() << endl; } cout << "Saving the map file. . ." << endl; MAP.lock(); MAP.write("map.ppm"); MAP.unlock(); cout << "Done!" << endl; } if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; return(0); }