//* Flood-based path finding algorithm class - implementation file *

#include "nav_path_flood.h" #include<vector> #include<iostream> #include<iomanip> void NAV_Path_Flood::addObstacle(double xo, double yo, short int probablity) { PATH.operator()(xo, yo) = VAL_OBSTACLE; // the middle of the obstacle // thicken obstacles but simplify - use square for robot contour instead of circle const double resolution = PATH.getResolution(); const int steps = rradius/resolution; if (steps==0) return; // the loops are expanded in four quadrants to optimize checks for out of range for (int i=0; i<=steps; ++i) { double x = xo + i*resolution; for (int j=0; j<=steps; ++j) { double y = yo + j*resolution; if (withinRange(x, y)) { PATH.operator()(x, y) = VAL_OBSTACLE; } else { break; } } for (int j=0; j<=steps; ++j) { double y = yo - j*resolution; if (withinRange(x, y)) { PATH.operator()(x, y) = VAL_OBSTACLE; } else { break; } } } for (int i=0; i<=steps; ++i) { double x = xo - i*resolution; for (int j=0; j<=steps; ++j) { double y = yo + j*resolution; if (withinRange(x, y)) { PATH.operator()(x, y) = VAL_OBSTACLE; } else { break; } } for (int j=0; j<=steps; ++j) { double y = yo - j*resolution; if (withinRange(x, y)) { PATH.operator()(x, y) = VAL_OBSTACLE; } else { break; } } } } void NAV_Path_Flood::register_map(NAV_Map *RobotMap) { // add only a new map for (size_t i=0; i<MAPS.size(); ++i) if (MAPS[i]==RobotMap) return; MAPS.push_back(RobotMap); } bool NAV_Path_Flood::unregister_map(NAV_Map *RobotMap) { std::vector<NAV_Map*>::iterator i; i=MAPS.begin(); ++i; // the very first map cannot be removed while (i!=MAPS.end()) { if (*i == RobotMap) { MAPS.erase(i); return(true); } ++i; } return(false); } void NAV_Path_Flood::recalculate() { #ifdef _DEBUG std::cerr<<"FLOOD: initialized"<<std::endl; #endif // initialize a new path computation matrix const double routresol = PATH.getResolution(); PATH.clear(); for (size_t i=0; i<MAPS.size(); ++i) { MAPS[i]->lock(); // make sure the range covers all maps, resize as needed for each map PATH.resize_as_needed(MAPS[i]->getMinX(), MAPS[i]->getMinY(), MAPS[i]->getMaxX(), MAPS[i]->getMaxY()); // map limits and grid step const double copyresol = MAPS[i]->getResolution(); const double xmin = PATH.getMinX(); const double xmax = PATH.getMaxX(); const double ymin = PATH.getMinY(); const double ymax = PATH.getMaxY(); // copy obstacles to the map #ifdef _DEBUG long int obst_cnt=0; std::cerr<<"FLOOD: M["<<i<<"] obst:"; #endif for (double x=xmin+copyresol; x<xmax; x=x+copyresol) { for (double y=ymin+copyresol; y<ymax; y=y+copyresol) { // will not lock continuously to allow updates thus prevent blocking // the robot control thread in case multithreading is used if (MAPS[i]->knowThere(x, y) ) { // flood - mark obstacles // potential field - add up obstacle to existing potential field // MAP->lock(); // use locking if map updated at the same time from another thread unsigned char probabl = MAPS[i]->obstThere(x, y); // MAP->unlock(); if (probabl>=0x40) { addObstacle(x, y, probabl); #ifdef _DEBUG ++obst_cnt; #endif } } } } #ifdef _DEBUG std::cerr<<std::setw(5)<<obst_cnt<<std::endl; #endif MAPS[i]->unlock(); } // calculate the paths using flooding algorithm { std::vector<myPoint> oldtocheck; std::vector<myPoint> newtocheck; count_t distance= VAL_START; // the first point { double x=static_cast<long int>(cgoal.getX()/routresol)*routresol; double y=static_cast<long int>(cgoal.getY()/routresol)*routresol; // Note: it is important that (x,y) is adjusted to the grid, // Otherwise, the flooding algorithm fails when crossing an axis PATH.resize_as_needed(x,y); // resize as needed to cover the goal PATH(x,y)=distance; if (toBeRouted(x-routresol,y)) newtocheck.push_back(myPoint(x-routresol, y)); if (toBeRouted(x+routresol,y)) newtocheck.push_back(myPoint(x+routresol, y)); if (toBeRouted(x,y-routresol)) newtocheck.push_back(myPoint(x, y-routresol)); if (toBeRouted(x,y+routresol)) newtocheck.push_back(myPoint(x, y+routresol)); } ++distance; do { oldtocheck.swap(newtocheck); // Note: oldtocheck=newtocheck runs much, much slower!!! for (size_t i=0; i<oldtocheck.size(); ++i) { double x=oldtocheck[i].getX(); double y=oldtocheck[i].getY(); if (PATH(x,y)==VAL_EMPTY) { PATH(x,y)=distance; if (toBeRouted(x-routresol,y)) newtocheck.push_back(myPoint(x-routresol, y)); if (toBeRouted(x+routresol,y)) newtocheck.push_back(myPoint(x+routresol, y)); if (toBeRouted(x,y-routresol)) newtocheck.push_back(myPoint(x, y-routresol)); if (toBeRouted(x,y+routresol)) newtocheck.push_back(myPoint(x, y+routresol)); } } ++distance; oldtocheck.clear(); // clear the vector to start over in the next iteration #ifdef _DEBUG std::cerr<<"\rFLOOD: "<<std::setw(7)<<distance<<" "<<std::setw(7)<<newtocheck.size()<<"\r"; #endif } while (newtocheck.size()>0); #ifdef _DEBUG std::cerr<<"FLOOD: max dist: "<<std::setw(5)<<distance<<std::endl; std::cerr<<"FLOOD: grid prepared 0"<<std::endl; #endif } } double NAV_Path_Flood::getHeading(double x, double y) const { const double dr = PATH.getResolution(); double heading = -360; double minimum = getValue(x,y); // needs to be optimized if (withinRange(x+dr, y) && getValue(x+dr,y)<minimum) { minimum=getValue(x+dr,y); heading=0; } if (withinRange(x+dr, y+dr) && getValue(x+dr,y+dr)<minimum) { minimum=getValue(x+dr,y+dr); heading=45; } if (withinRange(x, y+dr) && getValue(x,y+dr)<minimum) { minimum=getValue(x+dr,y); heading=90; } if (withinRange(x-dr, y+dr) && getValue(x-dr,y+dr)<minimum) { minimum=getValue(x-dr,y+dr); heading=135; } if (withinRange(x-dr, y) && getValue(x-dr,y)<minimum) { minimum=getValue(x-dr,y); heading=180; } if (withinRange(x-dr, y-dr) && getValue(x-dr,y-dr)<minimum) { minimum=getValue(x-dr,y-dr); heading=-135; } if (withinRange(x, y-dr) && getValue(x,y-dr)<minimum) { minimum=getValue(x,y-dr); heading=-90; } if (withinRange(x+dr, y-dr) && getValue(x+dr,y-dr)<minimum) { minimum=getValue(x+dr,y-dr); heading=-45; } return(heading); } double NAV_Path_Flood::getVelCoef(double x, double y) const { if (withinRange(x,y)) { double val = getValue(x,y); if (val<VAL_OBSTACLE) { if (val>VAL_CLOSE) return(1.0); // regular spped else return(0.25); // slow down close to the goal // consider: check if obstacles near by and slow down if so } else return(0.1); // goal reached or inside of an obstacle } return(0.0); // we are outside of the map -- cannot advise on direction } double NAV_Path_Flood::getOmegaCoef(double x, double y, double theta) const { double heading = getHeading(x,y); if (heading==-360) return(0.0); // cannot make it to the goal, don't move double omega = ArMath::fixAngle(heading - theta); // fixAngle puts it within (-180;180] // if (omega<=-180) omega+=360; else if (omega>180) omega-=360; // old fixAngle return((1./180.)*omega); }