//* Mapping with multithreading 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 <process.h> /* needed for _beginthread(...) */ #include <conio.h> /* needed for _kbhit() _getchar() */ using namespace std; /* controling when a thread ends */ /* volatile */ bool keep_running = true; // global variable that can be checked/changed as atomic transaction /* passing parameters to threads */ struct thread_params_map { // We can only pass one pointer to a thread NAV_Robot *r; // Dillema: perhaps we should simply use global NAV_Map *m; // variables instead passing pointers? }; /* parameters to control mapping functionality */ const long int map_timeStep = 500; // Minimum time step used for mapping const long int map_timeSave = 180000L; // Maximum time between saving map backups /* passing parameters to threads */ struct thread_params_drive { // We can only pass one pointer to a thread NAV_Robot *r; // Dillema: perhaps we should simply use global NAV_Map *m; // variables instead passing pointers? long int max_time; // Maximum time to drive or zero==forever }; /* 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 /* main program loop */ const long int kbd_timeStep = 50; // Minimum time step used for mapping void continuous_mappling(void * params) { thread_params_map * P = reinterpret_cast<thread_params_map*>(params); ArTime maptime; maptime.setToNow(); while(keep_running) { map_RobotBuffer(*(P->r), *(P->m)); /* this function locks both the robot and the map inside */ ArUtil::sleep(map_timeStep); if (_kbhit()) { char c=_getch(); if (tolower(c)=='s') { cout << "Saving the map file. . ."; (P->m)->lock(); (P->m)->write("map.ppm"); (P->m)->unlock(); maptime.setToNow(); cout << " Done!" << endl; } else { _ungetch(c); // it wasn't for this this thread } } if (maptime.mSecSince()>=map_timeSave) { cout << "Saving the map file. . ."; (P->m)->lock(); (P->m)->write("map.ppm"); (P->m)->unlock(); maptime.setToNow(); cout << " Done!" << endl; } } } /* drive_RandomWallToWall for max_time ms or forever if 0 */ void drive_RandomWallToWall(void * params) { #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif thread_params_drive * P = reinterpret_cast<thread_params_drive*>(params); // this code portion is for testing returning to the preset position only (has nothing to do with mapping) P->r->lock(); NAV_Proximity POINT( P->r->getPose(), P->r->getRobotRadius()*5 ); P->r->unlock(); // this code portion is for testing crossing a preset line only (has nothing to do with mapping) P->r->lock(); //NAV_LineCrossing SIDES( P->r->getX(), P->r->getY(), P->r->getX() + 1*ArMath::cos(P->r->getTh()), P->r->getY() + 1*ArMath::sin(P->r->getTh()) ); NAV_LineCrossing SIDES( P->r->getX(), P->r->getY(), P->r->getTh() ); P->r->unlock(); ArTime runtime; runtime.setToNow(); (P->r)->lock(); while(keep_running && (P->max_time ==0 || runtime.mSecSince()<P->max_time) ) { if ( !P->r->is_connected() ) { P->r->unlock(); keep_running=false; return; } if ( P->r->isStalled() ) { P->r->Stop(); P->r->unlock(); keep_running=false; return; } // this code portion is for testing returning to the preset position only (has nothing to do with mapping) if (POINT.checkReturned(P->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(P->r->getPose())) { cout << "*** crossed between left and right half plains ***" << endl; } if ( P->r->getDistanceAhead() < P->r->getRobotLengthFront()+300. || P->r->getSonarDistance(-100, 100) < P->r->getRobotRadius()+200. ) { P->r->doDriveSpeed(0); if ( P->r->getSonarDistance(-100, -10) < P->r->getSonarDistance(10, 100) ) P->r->doRotateSpeed(drive_rotateSpeed); else P->r->doRotateSpeed(-drive_rotateSpeed); P->r->unlock(); P->r->Sleep(drive_timeStep); P->r->lock(); while ( P->r->getDistanceAhead() < P->r->getRobotLengthFront()+300. || P->r->getSonarDistance(-100, 100) < P->r->getRobotRadius()+200. ) { P->r->unlock(); P->r->Sleep(drive_timeStep); P->r->lock(); } } P->r->doRotateSpeed(0) ; P->r->doDriveSpeed(drive_driveSpeed); P->r->unlock(); P->r->Sleep(drive_timeStep); P->r->lock(); #ifdef _DEBUG cerr <<"RNDW "<<" X="<<setw(6)<< P->r->getX() <<" Y="<<setw(6)<< P->r->getY() <<" T="<<setw(6)<< P->r->getTh() << endl; #endif if (_kbhit()) { char c=_getch(); if (tolower(c)=='r') { // ... } else if (c=='p') { // this code portion is for testing returning to the preset position only (has nothing to do with mapping) POINT.setPose(P->r->getPose()); // this code portion is for testing crossing a preset line only (has nothing to do with mapping) SIDES = NAV_LineCrossing( P->r->getX(), P->r->getY(), P->r->getTh() ); cout << "*** position and line remembered ***" << endl; } else { _ungetch(c); // it wasn't for this this thread } } } P->r->Stop(); P->r->unlock(); keep_running=false; } 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")) // still in single threaed - no need to lock yet 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; } thread_params_map PM; PM.r = &R; PM.m = &MAP; thread_params_drive PD; PD.r = &R; PD.m = &MAP; PD.max_time = 0; HANDLE threads[2] = {NULL}; threads[0] = (HANDLE) _beginthread( (void (*)(void *)) drive_RandomWallToWall, 0, &PD); threads[1] = (HANDLE) _beginthread( (void (*)(void *)) continuous_mappling, 0, &PM); while(keep_running) { if (_kbhit()) { char c=_getch(); if (tolower(c)=='q') keep_running=false; // stop threads else _ungetch(c); // it wasn't for this this thread } R.Sleep(kbd_timeStep); } WaitForMultipleObjects(2, threads, true, INFINITE); for (size_t i=0; i<2; i++) if (threads[i]!=NULL) CloseHandle(threads[i]); if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; cout << "Saving the map file. . ." << endl; MAP.write("map.ppm"); // in single threaed again - again no need to lock cout << "Done!" << endl; return(0); }