string MCDMFunction::getEncodedKey(Pose& p, int value) { string key; //value = 0 : encode everything //value = 1 : encode x,y,orientation, take first //value = 2 : encode x,y,orientation, take multiple time if(value == 0){ key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + to_string(p.getRange()) + "/" + to_string(p.getFOV()); }else if(value == 1){ key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + "1"; } else if (value ==2){ key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + "0"; } return key; }
geometry_msgs::Pose MessageConversions::poseToMessage(const Pose& pose) const { geometry_msgs::Pose message; const Eigen::Vector3d& position = pose.getPosition(); message.position.x = position[0]; message.position.y = position[1]; message.position.z = position[2]; const Eigen::Quaterniond& orientation = pose.getOrientation(); message.orientation.x = orientation.x(); message.orientation.y = orientation.y(); message.orientation.z = orientation.z(); message.orientation.w = orientation.w(); return message; }
pair<Pose,double> MCDMFunction::selectNewPose(EvaluationRecords *evaluationRecords) { Pose newTarget; double value = 0; unordered_map<string,double> evaluation = evaluationRecords->getEvaluations(); for(unordered_map<string,double>::iterator it = evaluation.begin(); it != evaluation.end(); it++){ string tmp = (*it).first; Pose p = evaluationRecords->getPoseFromEncoding(tmp); if(value <= (*it).second){ newTarget = p; value = (*it).second; }//else continue; } pair<Pose,double> result = make_pair(newTarget,value); // i switch x and y to allow debugging graphically looking the image cout << "New target : " << "x = "<<newTarget.getY() <<", y = "<< newTarget.getX() << ", orientation = " <<newTarget.getOrientation() << ", Evaluation: "<< value << endl; return result; }
// Input : ./mcdm_online_exploration_ros ./../Maps/map_RiccardoFreiburg_1m2.pgm 100 75 5 0 15 180 0.95 0.12 // resolution x y orientation range centralAngle precision threshold int main ( int argc, char **argv ) { auto startMCDM = chrono::high_resolution_clock::now(); ifstream infile; infile.open ( argv[1] ); // the path to the map double resolution = atof ( argv[2] ); // the resolution of the map double imgresolution = atof ( argv[10] ); // the resolution to use for the planningGrid and RFIDGrid dummy::Map map = dummy::Map ( infile,resolution, imgresolution ); RFIDGridmap myGrid(argv[1], resolution, imgresolution, false); cout << "Map dimension: " << map.getNumGridCols() << " : "<< map.getNumGridRows() << endl; int gridToPathGridScale = map.getGridToPathGridScale(); // i switched x and y because the map's orientation inside and outside programs are different long initX = static_cast<long>( atoi ( argv[4] ) *imgresolution ); // initial X-position of the robot in map frame long initY = static_cast<long>( atoi ( argv[3] ) *imgresolution ); // initial Y-position of the robot in map frame std::cout << "initX: " << initX << " initY: " << initY << std::endl; int initOrientation = atoi ( argv[5] ); // initial orientation of the robot in map frame double initFov = atoi ( argv[7] ); // initial FOV of the robot sensor initFov = initFov * PI /180; int initRange = atoi ( argv[6] ); double precision = atof ( argv[8] ); double threshold = atof ( argv[9] ); //x,y,orientation,range,FOV Pose initialPose = Pose ( initX,initY,initOrientation,initRange,initFov ); Pose invertedInitial = createFromInitialPose ( initX,initY,initOrientation,180,initRange,initFov ); Pose eastInitial = createFromInitialPose ( initX,initY,initOrientation,90,initRange,initFov ); Pose westInitial = createFromInitialPose ( initX,initY,initOrientation,270,initRange,initFov ); Pose target = initialPose; Pose previous = initialPose; long numConfiguration = 1; vector<pair<string,list<Pose>>> graph2; NewRay ray; ray.setGridToPathGridScale ( gridToPathGridScale ); MCDMFunction function; long sensedCells = 0; long newSensedCells =0; long totalFreeCells = map.getTotalFreeCells(); int count = 0; double travelledDistance = 0; int numOfTurning = 0; unordered_map<string,int> visitedCell; vector<string>history; history.push_back ( function.getEncodedKey ( target,1 ) ); EvaluationRecords record; //amount of time the robot should do nothing for scanning the environment ( final value expressed in second) unsigned int microseconds = 5 * 1000 * 1000 ; list<Pose> unexploredFrontiers; list<Pose> tabuList; tabuList.push_back(target); list<Pose> nearCandidates; bool btMode = false; double totalAngle = 0; Astar astar; double totalScanTime = 0; bool act = true; int encodedKeyValue = 0; // RFID double absTagX = std::stod(argv[12]); // m. double absTagY = std::stod(argv[11]); // m. double freq = std::stod(argv[13]); // Hertzs double txtPower= std::stod(argv[14]); // dBs std::pair<int, int> relTagCoord; do { // If we are doing "forward" navigation towards cells never visited before if ( btMode == false ) { long x = target.getX(); long y = target.getY(); int orientation = target.getOrientation(); int range = target.getRange(); double FOV = target.getFOV(); string actualPose = function.getEncodedKey ( target,0 ); map.setCurrentPose ( target ); string encoding = to_string ( target.getX() ) + to_string ( target.getY() ); visitedCell.emplace ( encoding,0 ); // Get the sensing time required for scanning target.setScanAngles ( ray.getSensingTime ( map,x,y,orientation,FOV,range ) ); // Perform a scanning operation newSensedCells = sensedCells + ray.performSensingOperation ( map,x,y,orientation,FOV,range, target.getScanAngles().first, target.getScanAngles().second ); // Calculate the scanning angle double scanAngle = target.getScanAngles().second - target.getScanAngles().first; // Update the overall scanning time totalScanTime += calculateScanTime ( scanAngle*180/PI ); // Calculare the relative RFID tag position to the robot position relTagCoord = map.getRelativeTagCoord(absTagX, absTagY, target.getX(), target.getY()); // Calculate the received power and phase double rxPower = received_power_friis(relTagCoord.first, relTagCoord.second, freq, txtPower); double phase = phaseDifference(relTagCoord.first, relTagCoord.second, freq); // Update the path planning and RFID map map.updatePathPlanningGrid ( x, y, range, rxPower - SENSITIVITY); myGrid.addEllipse(rxPower - SENSITIVITY, map.getNumGridCols() - target.getX(), target.getY(), target.getOrientation(), -0.5, 7.0); // Search for new candidate position ray.findCandidatePositions ( map,x,y,orientation,FOV,range ); vector<pair<long,long> >candidatePosition = ray.getCandidatePositions(); ray.emptyCandidatePositions(); // Old code // TODO: leave it, I don't know what it does // if ( act ) // { // //NOTE; calculate path and turnings between actual position and goal // /*string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // travelledDistance = travelledDistance + astar.lengthPath ( path ); // cout << astar.lengthPath ( path ) << endl; // numOfTurning = numOfTurning + astar.getNumberOfTurning ( path ); // numConfiguration++; // */ // //totalAngle += scanAngle; // } // act = false; // If the exploration just started if ( count == 0 ) { // Calculate other three pose given the strating one string invertedPose = function.getEncodedKey ( invertedInitial,0 ); string eastPose = function.getEncodedKey ( eastInitial,0 ); string westPose = function.getEncodedKey ( westInitial,0 ); list<Pose> empty ; std::pair<string,list<Pose>> pair1 = make_pair ( invertedPose,empty ); std::pair<string,list<Pose>> pair2 = make_pair ( eastPose,empty ); std::pair<string,list<Pose>> pair3 = make_pair ( westPose,empty ); // And add them (with empty candidates) to the graph structure graph2.push_back ( pair1 ); graph2.push_back ( pair2 ); graph2.push_back ( pair3 ); } // If it's not the first step but we are in one of the initial position (we come back here with backtracking) if ( count != 0 && ( target.isEqual ( invertedInitial ) || target.isEqual ( eastInitial ) || target.isEqual ( westInitial ) ) ) { // If there are no more destination in the graph, terminates the navigation if ( graph2.size() == 0 ) break; graph2.pop_back(); actualPose = function.getEncodedKey ( target,0 ); // Add to the graph the initial positions and the candidates from there (calculated inside the function) pushInitialPositions ( map, x, y,orientation, range,FOV, threshold, actualPose, &graph2 ); } // If there are no new candidate positions from the current pose of the robot if ( candidatePosition.size() == 0 ) { // Find candidates ray.findCandidatePositions2 ( map,x,y,orientation,FOV,range ); candidatePosition = ray.getCandidatePositions(); ray.emptyCandidatePositions(); cout << "No other candidate position" << endl; cout << "----- BACKTRACKING -----" << endl; // If the graph contains cells that can be explored if ( graph2.size() > 1 ) { // Get the last position in the graph and then remove it string targetString = graph2.at ( graph2.size()-1 ).first; graph2.pop_back(); // EvaluationRecords record; target = record.getPoseFromEncoding ( targetString ); // Add it to the history as cell visited more than once history.push_back ( function.getEncodedKey ( target,2 ) ); cout << "[BT]No significative position reachable. Come back to previous position" << endl; count = count + 1; } //...otherwise, if the graph does not contain cells that can be explored // The navigation is finished! else { cout << "Num configuration: " << numConfiguration << endl; cout << "Travelled distance calculated during the algorithm: " << travelledDistance << endl; cout << "------------------ HISTORY -----------------" << endl; // Retrieve the cell visited only the first time list<Pose> tmp_history = cleanHistory(&history, &record); calculateDistance(tmp_history, map, &astar ); cout << "------------------ TABULIST -----------------" << endl; // Calculate the path connecting the cells in the tabulist, namely the cells that are visited one time and couldn't be visite again calculateDistance(tabuList, map, &astar ); // Normalise the travel distance in meter // NOTE: assuming that the robot is moving at 0.5m/s and the resolution of the map is 0.5m per cell) if ( imgresolution == 1.0 ) { travelledDistance = travelledDistance/2; } printResult(newSensedCells, totalFreeCells, precision, numConfiguration, travelledDistance, numOfTurning, totalAngle, totalScanTime); exit ( 0 ); } sensedCells = newSensedCells; } //... otherwise, if there are further candidate new position from the current pose of the robot else { // need to convert from a <int,int pair> to a Pose with also orientation,laser range and angle list<Pose> frontiers; // For every candidate positio, create 8 pose with a different orientation each and consider them as frontiers vector<pair<long,long> >::iterator it =candidatePosition.begin(); for ( it; it != candidatePosition.end(); it++ ) { Pose p1 = Pose ( ( *it ).first, ( *it ).second,0 ,range,FOV ); Pose p2 = Pose ( ( *it ).first, ( *it ).second,45,range,FOV ); Pose p3 = Pose ( ( *it ).first, ( *it ).second,90,range,FOV ); Pose p4 = Pose ( ( *it ).first, ( *it ).second,135,range,FOV ); Pose p5 = Pose ( ( *it ).first, ( *it ).second,180,range,FOV ); Pose p6 = Pose ( ( *it ).first, ( *it ).second,225,range,FOV ); Pose p7 = Pose ( ( *it ).first, ( *it ).second,270,range,FOV ); Pose p8 = Pose ( ( *it ).first, ( *it ).second,315,range,FOV ); frontiers.push_back ( p1 ); frontiers.push_back(p2); frontiers.push_back ( p3 ); frontiers.push_back(p4); frontiers.push_back ( p5 ); frontiers.push_back(p6); frontiers.push_back ( p7 ); frontiers.push_back(p8); } unexploredFrontiers = frontiers; // Evaluate the frontiers and return a list of <frontier, evaluation> pairs EvaluationRecords *record = function.evaluateFrontiers ( frontiers,map,threshold ); nearCandidates = record->getFrontiers(); // If there are candidate positions if ( record->size() != 0 ) { // Set the previous pose equal to the current one (represented by target) previous = target; // Select the new robot destination from the list of candidates std::pair<Pose,double> result = function.selectNewPose ( record ); target = result.first; // If the selected destination does not appear among the cells already visited if ( ! contains ( tabuList,target )) { act = true; // Add it to the list of visited cells as first-view encodedKeyValue = 1; updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function, &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle); // count = count + 1; // // numConfiguration++; // // cout << "ADDED" << x << " " << y << endl; // // Add it to the list of visited cells as first-view // history.push_back ( function.getEncodedKey ( target,1 ) ); // cout << function.getEncodedKey ( target,1 ) << endl; // // Add it to the list of visited cells from which acting // tabuList.push_back ( target ); // // Remove it from the list of candidate position // cleanPossibleDestination2 ( nearCandidates,target ); // // Push in the graph the previous robot pose and the new list of candidate position, without the current pose of the robot // // We don't want to visit this cell again // std::pair<string,list<Pose>> pair = make_pair ( actualPose,nearCandidates ); // graph2.push_back ( pair ); // // Calculate the path from the previous robot pose to the current one // string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // cout << "1: " << travelledDistance << endl; // // Update the distance counting // travelledDistance = travelledDistance + astar.lengthPath(path); // cout << "2: " << travelledDistance << endl; // // Update the turning counting // numOfTurning = numOfTurning + astar.getNumberOfTurning(path); // // Update the number of configurations of the robot along the task // numConfiguration++; // // Update the scanned angle counting // totalAngle += scanAngle; } // ...otherwise, if the seleced cell has already been visited else { // If the graph is empty, stop the navigation if ( graph2.size() == 0 ) break; // If there still are more candidates to explore from the last pose in the graph if ( graph2.at ( graph2.size()-1 ).second.size() != 0 ) { cout << "[BT1 - Tabulist]There are visible cells but the selected one is already explored!Come back to second best position from the previous position"<< endl; // Remove the current position from possible candidates cleanPossibleDestination2 ( nearCandidates,target ); // Get the list of new candidate position with associated evaluation record = function.evaluateFrontiers ( nearCandidates,map,threshold ); // If there are candidate positions if ( record->size() != 0 ) { // Select the new pose of the robot std::pair<Pose,double> result = function.selectNewPose ( record ); target = result.first; encodedKeyValue = 1; updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function, &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle); // // Add it to the list of visited cell to not go back // tabuList.push_back ( target ); // // Add it to history of cell as visited for the first time // history.push_back ( function.getEncodedKey ( target,1 ) ); // cout << function.getEncodedKey ( target,1 ) << endl; // // Calculate the path from current robot pose to there // string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // cout << "1: " << travelledDistance << endl; // // Update the overall distance covered by the robot // travelledDistance = travelledDistance + astar.lengthPath(path); // cout << "2: " << travelledDistance << endl; // // Update the overall number of turnings // numOfTurning = numOfTurning + astar.getNumberOfTurning(path); // // Update the number of configurations of the robot during the navigation // numConfiguration++; // // Update the overall scan angle // totalAngle += scanAngle; // count = count + 1; // Set that we are now in backtracking btMode = true; } // If there are no more candidate position from the last position in the graph else { // if the graph is now empty, stop the navigation if ( graph2.size() == 0 ) break; // Otherwise, select as new position the last cell in the graph and then remove it from there string targetString = graph2.at ( graph2.size()-1 ).first; graph2.pop_back(); target = record->getPoseFromEncoding ( targetString ); } } // ... if the graph still does not present anymore candidate positions for its last pose else { // Remove the last element (cell and associated candidate from there) from the graph graph2.pop_back(); // Select as new target, the new last element of the graph string targetString = graph2.at ( graph2.size()-1 ).first; target = record->getPoseFromEncoding ( targetString ); // Save it history as cell visited more than once history.push_back ( function.getEncodedKey ( target,2 ) ); cout << "[BT2 - Tabulist]There are visible cells but the selected one is already explored!Come back to two position ago"<< endl; count = count + 1; } } } // ... otherwise, if there are no candidate positions else { // If the graph is empty, stop the navigation if ( graph2.size() == 0 ) break; // Select as new target the last one in the graph structure string targetString = graph2.at ( graph2.size()-1 ).first; // Remove it from the graph graph2.pop_back(); target = record->getPoseFromEncoding ( targetString ); // Check if the selected cell in the graph is the previous robot position if ( !target.isEqual ( previous ) ) { // if it's not, set the old position as the current one previous = target; //TODO: WHY? cout << "[BT3]There are no visible cells so come back to previous position in the graph structure" << endl; // Save the new target in the history as cell visited more than once history.push_back ( function.getEncodedKey ( target,2 ) ); count = count + 1; } // If the selected cell is the old robot position else { // If there are no more cells in the graph, just finish the navigation if ( graph2.size() == 0 ) { cout << "[BT4]No other possibilities to do backtracking on previous positions" << endl; break; } // Select the last position in the graph string targetString = graph2.at ( graph2.size()-1 ).first; // and remove it from the graph graph2.pop_back(); target = record->getPoseFromEncoding ( targetString ); // Set the previous pose as the current one previous = target; cout << "[BT5]There are no visible cells so come back to previous position" << endl; cout << "[BT5]Cell already explored!Come back to previous position"<< endl; // Add it in history as cell visited more than once history.push_back ( function.getEncodedKey ( target,2 ) ); count = count + 1; } } //NOTE: not requested for testing purpose //usleep(microseconds); sensedCells = newSensedCells; frontiers.clear(); candidatePosition.clear(); delete record; } } // ... otherwise, if we are doing backtracking else { long x = target.getX(); long y = target.getY(); int orientation = target.getOrientation(); int range = target.getRange(); double FOV = target.getFOV(); string actualPose = function.getEncodedKey ( target,0 ); map.setCurrentPose ( target ); //NOTE; calculate path and turnings between actual position and goal cout<< function.getEncodedKey ( target,1 ) << endl; // Calculate the distance between the previous robot pose and the next one (target) string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // Update the overall covered distance travelledDistance = travelledDistance + astar.lengthPath ( path ); cout << "BT: " << astar.lengthPath ( path ) << endl; // Update the overall number of turnings numOfTurning = numOfTurning + astar.getNumberOfTurning ( path ); string encoding = to_string ( target.getX() ) + to_string ( target.getY() ); visitedCell.emplace ( encoding,0 ); // Set the previous cell to be the same of the current one previous = target; // Calculate how much time it takes to scan the current area target.setScanAngles ( ray.getSensingTime ( map,x,y,orientation,FOV,range ) ); // Get the scanning angle double scanAngle = target.getScanAngles().second - target.getScanAngles().first; // Update the overall scanned angle totalAngle += scanAngle; // ...and the overall scan time totalScanTime += calculateScanTime ( scanAngle*180/PI ); // Calculate the relative coordinate to the robot of the RFID tag relTagCoord = map.getRelativeTagCoord(absTagX, absTagY, target.getX(), target.getY()); // Calculate received power and phase double rxPower = received_power_friis(relTagCoord.first, relTagCoord.second, freq, txtPower); double phase = phaseDifference(relTagCoord.first, relTagCoord.second, freq); map.updatePathPlanningGrid ( x, y, range, rxPower - SENSITIVITY ); myGrid.addEllipse(rxPower - SENSITIVITY, map.getNumGridCols() - target.getX(), target.getY(), target.getOrientation(), -0.5, 7.0); // Remove the current pose from the list of possible candidate cells cleanPossibleDestination2 ( nearCandidates,target ); // Get the list of the candidate cells with their evaluation EvaluationRecords *record = function.evaluateFrontiers ( nearCandidates,map,threshold ); // If there are candidate cells if ( record->size() != 0 ) { // Find the new destination std::pair<Pose,double> result = function.selectNewPose ( record ); target = result.first; // If this cells has not been visited before if ( ! contains ( tabuList,target ) ) { // Add it to the list of visited cells as first-view encodedKeyValue = 1; updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function, &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle); // // // Add it to history as visited for the first time // history.push_back ( function.getEncodedKey ( target,1 ) ); // // Add it to cell that must not be visited again // tabuList.push_back ( target ); // cout<< function.getEncodedKey ( target,1 ) << endl; // // Calculate the distance between previous pose to the new one // string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // // Update the overall counting of distance // travelledDistance = travelledDistance + astar.lengthPath(path); // cout << "BTL "<<astar.lengthPath ( path ) << endl; // // Update the counting of turning // numOfTurning = numOfTurning + astar.getNumberOfTurning(path); // // Update the counting of configuration of the robot during the navigation // numConfiguration++; // // Update the overall scanned angle // totalAngle += scanAngle; // // Remove the destination from the list of candidate cells // cleanPossibleDestination2 ( nearCandidates,target ); // // Associate the list of candiates to the current pose // std::pair<string,list<Pose>> pair = make_pair ( actualPose,nearCandidates ); // count = count + 1; // Leave the backtracking branch btMode = false; nearCandidates.clear(); cout << "[BT-MODE4] Go back to previous positions in the graph" << endl; } // ... otherwise, if the cells has already been visisted else { // If there are other candidates if ( nearCandidates.size() != 0 ) { cout << "[BT-MODE1]Already visited, but there are other candidates" << endl; // Remove the destination from the candidate list cleanPossibleDestination2 ( nearCandidates,target ); // Get the candidates with their evaluation EvaluationRecords *record = function.evaluateFrontiers ( nearCandidates,map,threshold ); // Select the new destination std::pair<Pose,double> result = function.selectNewPose ( record ); target = result.first; // Add it to the list of visited cells as first-view encodedKeyValue = 1; updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function, &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle); // // Add it to the list of cell that must not be visited again // tabuList.push_back ( target ); // history.push_back ( function.getEncodedKey ( target,1 ) ); // added recently but it was not here // cout<< function.getEncodedKey ( target,1 ) << endl; // // Calculate the distance between the destination and the previous pose // string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // cout << "BT :"<<astar.lengthPath ( path ) << endl; // // Update the overall distance covered by the robot // travelledDistance = travelledDistance + astar.lengthPath(path); // // Update the overall number of turnings // numOfTurning = numOfTurning + astar.getNumberOfTurning(path); // // Update the number of configuration of the robot during navigation // numConfiguration++; // // Update the overall scanned angle // totalAngle += scanAngle; // count = count + 1; } // ...otherwise, if there are no more candidates else { cout << "[BT-MODE2] Go back to previous positions in the graph" << endl; // Select as target the last element in the graph string targetString = graph2.at ( graph2.size()-1 ).first; // And remove from the graph graph2.pop_back(); target = record->getPoseFromEncoding ( targetString ); // Add it to the history of cell as already more than once encodedKeyValue = 2; updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function, &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle); // // Add it to the list of cells that must not be visited again // tabuList.push_back ( target ); // // Add it to the history of cell as already more than once // history.push_back ( function.getEncodedKey ( target,2 ) ); // // Calculate the distance from the previous pose and the destination // string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // cout <<"BT: "<< astar.lengthPath ( path ) << endl; // // Update the overall distance of the robot // travelledDistance = travelledDistance + astar.lengthPath(path); // // Update the overall number of turnings // numOfTurning = numOfTurning + astar.getNumberOfTurning(path); // // Update the overall number of configuration of the robot during navigation // numConfiguration++; // // Update the overall scanned angle // totalAngle += scanAngle; // count++; // Leave backtracking btMode = false; // Clear candidate list nearCandidates.clear(); } } } // ... if there are not candidate cells else { // Select as new pose, the last cell in the graph string targetString = graph2.at ( graph2.size()-1 ).first; // and the remove it form the graph graph2.pop_back(); target = record->getPoseFromEncoding ( targetString ); // Add it in history as cell visited more than once encodedKeyValue = 2; updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function, &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle); // // Add it to the list of cell that must not be visited again // tabuList.push_back ( target ); // // Add it in history as cell visited more than once // history.push_back ( function.getEncodedKey ( target,2 ) ); // // Calculate the distance from the previous cell to the new destination // string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map ); // cout <<"BT:"<< astar.lengthPath ( path ) << endl; // // Update the overall distance covered by the robot // travelledDistance = travelledDistance + astar.lengthPath(path); // // Update the oevrall number of turning of the robot // numOfTurning = numOfTurning + astar.getNumberOfTurning(path); // // Update the number of configuration of the robot during navigation // numConfiguration++; // // Update the overall scanned angle // totalAngle += scanAngle; // count = count + 1; // Leave backtracking btMode = false; cout << "[BT-MODE3] Go back to previous positions in the graph" << endl; } delete record; } } // Perform exploration until a certain coverage is achieved while ( sensedCells < precision * totalFreeCells ); // Plotting utilities map.drawVisitedCells (); map.printVisitedCells ( history ); map.drawRFIDScan(); map.drawRFIDGridScan(myGrid); myGrid.saveAs(("/home/pulver/Desktop/MCDM/rfid_result_gridmap.pgm")); cout << "Num configuration: " << numConfiguration << endl; cout << "Travelled distance calculated during the algorithm: " << travelledDistance << endl; cout << "------------------ HISTORY -----------------" << endl; // Calculate which cells have been visited only once list<Pose> tmp_history = cleanHistory(&history, &record); calculateDistance(tmp_history, map, &astar ); cout << "------------------ TABULIST -----------------" << endl; calculateDistance(tabuList, map, &astar ); // Trasform distance in meters if ( imgresolution == 1.0 ) // Corridor map has a resolution of 0.5 meter per cell { travelledDistance = travelledDistance/2; } printResult(newSensedCells, totalFreeCells, precision, numConfiguration, travelledDistance, numOfTurning, totalAngle, totalScanTime); // Find the tag std::pair<int,int> tag = map.findTag(); cout << "RFID pose: [" << tag.second << "," << tag.first << "]" << endl; tag = map.findTagfromGridMap(myGrid); cout << "[Grid]RFID pose: [" << tag.second << "," << tag.first << "]" << endl; cout << "-----------------------------------------------------------------"<<endl; auto endMCDM= chrono::high_resolution_clock::now(); double totalTimeMCDM = chrono::duration<double,milli> ( endMCDM -startMCDM ).count(); // cout << "Total time for MCDM algorithm : " << totalTimeMCDM << "ms, " << totalTimeMCDM/1000 <<" s, " << // totalTimeMCDM/60000 << " m "<< endl; }
string EvaluationRecords::getEncodedKey(Pose& p) { string key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( (int)p.getOrientation()) + "/" + to_string(p.getRange()) +"/" + to_string(p.getFOV()); return key; }