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

#ifndef _NAV_MAP_FLOOD #define _NAV_MAP_FLOOD #include "../Aria/include/Aria.h" #include "nav_map_baset.h" #include "nav_map.h" #include <vector> class NAV_Path_Flood { public: typedef short int count_t; const static count_t VAL_OBSTACLE = MAXSHORT; const static count_t VAL_START = 1; const static count_t VAL_EMPTY = 0; public: NAV_Path_Flood (NAV_Map *RobotMap, const ArPose &goal=ArPose(), double resol=100, double robotradius=200) : MAPS(0), PATH(resol, 1, VAL_EMPTY), rradius(robotradius), cgoal(goal), myMutex(), VAL_CLOSE(2+static_cast<count_t>(3*robotradius/resol)) { 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(); // void setUseGoal(bool use) { /* use always */ } public: count_t getValue (const ArPose &pos) const { return(getValue(pos.getX(), pos.getY())); } count_t getValue (double x, double y) const { return(PATH.operator()(x, y)); } double getResolution() const { return(PATH.getResolution()); } double getMinX () const { return(PATH.getMinX()); } double getMaxX () const { return(PATH.getMaxX()); } double getMinY () const { return(PATH.getMinY()); } double getMaxY () const { return(PATH.getMaxY()); } const ArPose& getGoal () const { return(cgoal); } public: // ArPose interface bool is_reachable(const ArPose &pos) const { return(is_reachable(pos.getX(), pos.getY())); } bool is_goal (const ArPose &pos) const { return(is_goal(pos.getX(), pos.getY())); } bool is_close (const ArPose &pos) const { return(is_close(pos.getX(), pos.getY())); } double getHeading (const ArPose &pos) const { return(getHeading(pos.getX(), pos.getY())); } double getOmegaCoef(const ArPose &pos) const { return(getOmegaCoef(pos.getX(), pos.getY(),pos.getTh())); } double getVelCoef (const ArPose &pos) const { return(getVelCoef(pos.getX(), pos.getY())); } // direct coordiantes interface bool is_reachable(double x, double y) const { return( withinRange(x,y) && PATH(x,y)!=VAL_EMPTY ); } bool is_goal (double x, double y) const { return( withinRange(x,y) && PATH(x,y)==VAL_START ); } bool is_close (double x, double y) const { return( is_reachable(x,y)&& PATH(x,y)<=VAL_CLOSE ); } double getHeading (double x, double y) const; double getVelCoef (double x, double y) const; double getOmegaCoef(double x, double y, double theta) const; 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 NAV_Map_Base<count_t> PATH; // array with flood or potential field data 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 count_t VAL_CLOSE; // number of grid units to activate "robot close to" behavior protected: void addObstacle (double xo, double yo, short int probablity); bool withinRange (double xt, double yt) const { return( xt>PATH.getMinX() && xt<PATH.getMaxX() && yt>PATH.getMinY() && yt<PATH.getMaxY() ); } bool toBeRouted (double xt, double yt) const { return( withinRange(xt,yt) && PATH(xt,yt)==VAL_EMPTY ); } private: class myPoint { public: myPoint() : myX(0), myY(0) {} myPoint(double x, double y) : myX(x), myY(y) {} double getX() const { return(myX); } double getY() const { return(myY); } private: double myX, myY; }; }; #endif