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

#ifndef _NAV_MAP_POTENTIAL #define _NAV_MAP_POTENTIAL #include "../Aria/include/Aria.h" #include "nav_map_baset.h" #include "nav_map.h" #include <vector> class NAV_FIELD_Potential { public: NAV_FIELD_Potential (NAV_Map *RobotMap, const ArPose &goal=ArPose(), double resol=100, double robotradius=200) : MAPS(0), rradius(robotradius), cgoal(goal), ugoal(true), myMutex(), VAL_CLOSE(2+static_cast<double>(3*robotradius/resol)), Katr(1), Kreprel(1E4), Rrep(10*rradius), Rrep_inv(1/Rrep), cache_is_valid(false) { MAPS.push_back(RobotMap); recalculate(); } public: void register_map(NAV_Map *RobotMap); bool unregister_map(NAV_Map *RobotMap); void setGoal(const ArPose &goal) { cgoal=goal; recalculate(); } void recalculate() { cache_is_valid=false; /* otherwise, do nothing */ } void setUseGoal(bool use) { cache_is_valid=(ugoal==use); ugoal=use; } public: double getResolution() const { return(MAPS[0]->getResolution()); } const ArPose& getGoal () const { return(cgoal); } public: // ArPose interface // bool is_reachable(const ArPose &pos) const { return(???); } bool is_goal (const ArPose &pos) const { return( cgoal.findDistanceTo(pos)<=MAPS[0]->getResolution() ); } bool is_close (const ArPose &pos) const { return( cgoal.findDistanceTo(pos)<=VAL_CLOSE ); } double getValue (const ArPose &pos) { return( getValue(pos.getX(), pos.getY()) ); } double getHeading (const ArPose &pos) { return( getHeading(pos.getX(), pos.getY()) ); } double getOmegaCoef(const ArPose &pos) { return( getOmegaCoef(pos.getX(), pos.getY(),pos.getTh()) ); } double getVelCoef (const ArPose &pos) { return( getVelCoef(pos.getX(), pos.getY()) ); } // direct coordiantes interface // bool is_reachable(double x, double y) const { return( ??? ) } bool is_goal (double x, double y) const { return( is_goal(ArPose(x,y)) ); } bool is_close (double x, double y) const { return( is_close(ArPose(x,y)) ); } double getValue (double x, double y); double getHeading (double x, double y); double getVelCoef (double x, double y); double getOmegaCoef(double x, double y, double theta); protected: void calculate_at(double x, double y); // this actually does all calculations and updates the cached result public: // you must lock the map before using it and unlock after you are done with it if using in multithreaded application void lock () { myMutex.lock(); } void unlock () { myMutex.unlock(); } protected: std::vector<NAV_Map*> MAPS; // pointer to at least one or possibly more maps kept (and updated) by the robot bool ugoal; // use goal feature and attraction (true) or not (false) ArPose cgoal; // current goal used for navigation double rradius; // current robot radius to enlarge obstacles, set to 0 to disable ArMutex myMutex; // lock/unlock for multithreading double VAL_CLOSE; // number of grid units to activate "robot close to" behavior double Katr, Kreprel; // coefficients for attraction and repulsion fields double Rrep, Rrep_inv; // maximum distance for repulsion to be experienced // cached last calculated value bool cache_is_valid; double cache_x, cache_y; double cache_e, cache_dx, cache_dy; }; #endif