//* Potential field-based path finding algorithm class - implementation file *

#include "nav_field_potential.h" #include<vector> #include<iostream> #include<iomanip> inline double sqr(double x) { return(x*x); } void NAV_FIELD_Potential::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_FIELD_Potential::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); } double NAV_FIELD_Potential::getValue(double x, double y) { calculate_at(x,y); return(cache_e); } double NAV_FIELD_Potential::getHeading(double x, double y) { calculate_at(x,y); return( ArMath::fixAngle(ArMath::atan2(cache_dy,cache_dx) ) ); } double NAV_FIELD_Potential::getVelCoef(double x, double y) { calculate_at(x,y); double coef=cache_dx*cache_dx+cache_dy*cache_dy; coef*=1E-6; if (coef>1) coef=1; return(coef); } double NAV_FIELD_Potential::getOmegaCoef(double x, double y, double theta) { 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); } void NAV_FIELD_Potential::calculate_at(double x, double y) { if ( (!cache_is_valid) || (x!=cache_x) || (y!=cache_y) ) { double sum_pot = 0.0; double sum_dx = 0.0; double sum_dy = 0.0; // compute this block if attraction force is used if (ugoal) { const double goal_x = cgoal.getX(); const double goal_y = cgoal.getY(); const double goal_dist = (sqr(x-goal_x)+sqr(y-goal_y)); // Textbook EQ 6.5 const double goal_pot = 0.5*Katr*goal_dist; // Textbook EQ 6.5 continued const double goal_dx = -Katr*(x-goal_x); // Textbook EQ 6.7 const double goal_dy = -Katr*(y-goal_y); // Textbook EQ 6.8 sum_pot += goal_pot; sum_dx += goal_dx; sum_dy += goal_dy; } else { sum_pot += 1; // sum_dx += 0; sum_dy += 0; } const double Krep = Kreprel*sum_pot; // make K repulsion significant at the current point of attraction for (size_t i=0; i<MAPS.size(); ++i) { MAPS[i]->lock(); const double scanresol = MAPS[i]->getResolution(); const double xmin=x-Rrep; const double xmax=x+Rrep; const double ymin=y-Rrep; const double ymax=y+Rrep; for (double xo=xmin; xo<=xmax; xo=xo+scanresol) { for (double yo=ymin; yo<=ymax; yo=yo+scanresol) { if (MAPS[i]->knowThere(xo, yo) ) { unsigned char probabl = MAPS[i]->obstThere(xo, yo); if (probabl>=0x40) { const double obst_dist = sqrt(sqr(xo-x)+sqr(yo-y)); if (obst_dist==0.0) continue; // cannot be here, but just in case to prevent 1/0 if (obst_dist>Rrep) continue; // too far away from this obstace const double Rpot = 0.5*Krep*sqr(1/obst_dist -Rrep_inv); // Textbook EQ 6.09 const double part = sqr(x) -2*x*xo + sqr(xo) + sqr(y) - 2*y*yo +sqr(yo); const double isrp = 1/sqrt(part); const double srp3 = sqrt(part*part*part); const double drx = Krep*(x-xo)*(isrp-Rrep_inv) / srp3; // -D(Rpot)/dx | EQ const double dry = Krep*(y-yo)*(isrp-Rrep_inv) / srp3; // -D(Rpot)/dy | 6.10 sum_pot += Rpot; sum_dx += drx; sum_dy += dry; // std::cerr << "At ("<<x<<","<<y<<") obstacle ("<<xo<<","<<yo<<")"<<std::endl; } } } } MAPS[i]->unlock(); } cache_is_valid=false; cache_e = sum_pot; cache_dx = sum_dx; cache_dy = sum_dy; cache_x=x; cache_y=y; cache_is_valid=true; } }