//* Thread safe robot mapping support class header file *

#ifndef _NAV_MAP #define _NAV_MAP #include "../Aria/include/Aria.h" #include "nav_map_baset.h"

//* Dynamic Map Container *

class NAV_Map : public NAV_Map_Base<unsigned short int> { public: NAV_Map (double resol=100, double robotradius=150, double sensorrange=2500); 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(); } public: void addObstacle (const ArPose &point); // updates point to be more likely ocupied void addFreePoint (const ArPose &point); // updates point to be more likely free void addFreePointR (const ArPose &point); // updates point as free and add Robot trace void addFreePointAR (const ArPose &point); // updates point as free and add Robot trace void addLineToObst (const ArPose &echo, const ArPose &from); // and line of free leading to obstacle point void addLineToObstR (const ArPose &echo, const ArPose &robot); // and line of free leading from robot to obstacle point void addLineToObstAR (const ArPose &echo, const ArPose &robot); // and line of free leading from robot to obstacle point void addLineToFree (const ArPose &none, const ArPose &from); // add line of free leading to free point void addLineToFreeR (const ArPose &none, const ArPose &robot); // add line of free leading from robot to free point void addLineToFreeAR (const ArPose &none, const ArPose &robot); // add line of free leading from robot to free point void setCustoms (const ArPose &point, unsigned char s) { setCustoms(point.getX(),point.getY(),s); } // sets 5-bit custom value public: double getRobotRadius () const { return(res*rradius); } double getSensorRange () const { return(res*ssrange); } unsigned short getPoint (const ArPose &point) { return(getPoint(getPoint(point.getX(),point.getY()))); } unsigned char obstThere (const ArPose &point) { return(maskObstacle(getPoint(point.getX(),point.getY())));} bool beenThere (const ArPose &point) { return(maskRobot(getPoint(point.getX(),point.getY()))); } bool someThere (const ArPose &point) { return(maskARobots(getPoint(point.getX(),point.getY()))); } bool knowThere (const ArPose &point) { return(maskUnknown(getPoint(point.getX(),point.getY()))); } unsigned char getCustoms (const ArPose &point) { return(maskCustoms(getPoint(point.getX(),point.getY()))); } // for raw point see URAXXXXXPPPPPPPP unsigned short getPoint (double x, double y); // get the raw value of the point unsigned char obstThere (double x, double y) { return(maskObstacle(getPoint(x,y))); } // in probability value from 0 to 255 bool beenThere (double x, double y) { return(maskRobot(getPoint(x,y))!=0); } // remember that robot marks its radius! bool someThere (double x, double y) { return(maskARobots(getPoint(x,y))!=0); } // remember that each robot marks a radius! bool knowThere (double x, double y) { return(maskUnknown(getPoint(x,y))==0); } // if the point has really defined value public: void setCustoms (double x, double y, unsigned char s); // sets 5-bit custom value void andCustoms (double x, double y, unsigned char s); // sets 5-bit custom value void orrCustoms (double x, double y, unsigned char s); // sets 5-bit custom value void xorCustoms (double x, double y, unsigned char s); // sets 5-bit custom value void clrCustoms (double x, double y) { andCustoms(x,y,0); } // sets 5-bit custom value unsigned char getCustoms (double x, double y) { return(maskCustoms(getPoint(x,y))>>8); } // test user defined information void clrCustom4 (double x, double y) { andCustoms(x,y,0x0F); } // sets user defined information void clrCustom3 (double x, double y) { andCustoms(x,y,0x17); } // sets user defined information void clrCustom2 (double x, double y) { andCustoms(x,y,0x1B); } // sets user defined information void clrCustom1 (double x, double y) { andCustoms(x,y,0x1D); } // sets user defined information void clrCustom0 (double x, double y) { andCustoms(x,y,0x1E); } // sets user defined information void setCustom4 (double x, double y) { orrCustoms(x,y,0x10); } // sets user defined information void setCustom3 (double x, double y) { orrCustoms(x,y,0x08); } // sets user defined information void setCustom2 (double x, double y) { orrCustoms(x,y,0x04); } // sets user defined information void setCustom1 (double x, double y) { orrCustoms(x,y,0x02); } // sets user defined information void setCustom0 (double x, double y) { orrCustoms(x,y,0x01); } // sets user defined information bool getCustom4 (double x, double y) { return(maskCustom4(getPoint(x,y))!=0); } // test user defined information bool getCustom3 (double x, double y) { return(maskCustom3(getPoint(x,y))!=0); } // test user defined information bool getCustom2 (double x, double y) { return(maskCustom2(getPoint(x,y))!=0); } // test user defined information bool getCustom1 (double x, double y) { return(maskCustom1(getPoint(x,y))!=0); } // test user defined information bool getCustom0 (double x, double y) { return(maskCustom0(getPoint(x,y))!=0); } // test user defined information public: void clear (); // OVERWR: clears and resizes to the initial size bool write (const char* fname, unsigned char mask=0x7F, bool ascii=false); // writes the map encoded as a P6 or P3 PPM file bool read (const char* fname, unsigned char mask=0x7F); // reads the map encoded as a P6 (binary) or P3 (ASCII) PPM file public: // the following static functions are object value independent (they are just functions immeresed in the class) static unsigned short getValMaxObstacle () { return( 0x00FF); } // get the value of 100% probability static unsigned short getValRobot () { return( 0x6000); } // get the bit mask to set this robot trace point status static unsigned short getValARobots () { return( 0x4000); } // get the bit mask to set all robots trace point status static unsigned short getValUnknown () { return( 0x8000); } // get the bit mask to set unknown state status static unsigned short maskUnknown (unsigned short p) { return(p&0x8000); } // returns masked bits if no robot ever saw this point static unsigned short maskObstacle (unsigned short p) { return(p&0x00FF); } // remove the unknown and robot status initial value, returns 0..63 static unsigned short maskRobot (unsigned short p) { return(p&0x2000); } // returns masked bits if this robot was here, 0 otherwise static unsigned short maskARobots (unsigned short p) { return(p&0x6000); } // returns masked bits if any robot was here, 0 otherwise static unsigned short maskStatuses (unsigned short p) { return(p&0x7F00); } // returns masked all user defined status bits and robots public: // user defined information masks static unsigned short getValCustom4 () { return( 0x1000); } // user defined information static unsigned short getValCustom3 () { return( 0x0800); } // user defined information static unsigned short getValCustom2 () { return( 0x0400); } // user defined information static unsigned short getValCustom1 () { return( 0x0200); } // user defined information static unsigned short getValCustom0 () { return( 0x0100); } // user defined information static unsigned short maskCustoms (unsigned short p) { return(p&0x1F00); } // masks user defined bits static unsigned short maskCustom4 (unsigned short p) { return(p&0x1000); } // masks user defined bit static unsigned short maskCustom3 (unsigned short p) { return(p&0x0800); } // masks user defined bit static unsigned short maskCustom2 (unsigned short p) { return(p&0x0400); } // masks user defined bit static unsigned short maskCustom1 (unsigned short p) { return(p&0x0200); } // masks user defined bit static unsigned short maskCustom0 (unsigned short p) { return(p&0x0100); } // masks user defined bit protected: // code the Markov probability rules for map update below // URAXXXXXPPPPPPPP U - undefined, R - robot was here, PPPPPPPP - probability of obstace 0..255 static void adjustToOcupValue (unsigned short & p); static void adjustToFreeValue (unsigned short & p); static void adjustToRobotValue (unsigned short & p); static void adjustToARobotsValue (unsigned short & p); protected: void resizeRaw (int ix, int iy); // OVERWR: resize the map to have full horizon range around void adjustToOcupPoint (int ix, int iy); // adjusts the point to acupied void adjustToFreePoint (int ix, int iy); // adjusts the point to free void adjustToFreewRobotPoint (int ix, int iy); // marks the robot trace and adjusts the point to free void adjustToFreewARobotsPoint (int ix, int iy); // marks the robot trace and adjusts the point to free void adjustToFreewRobotPointAdj (int ix, int iy); // marks the robot trace with void adjustToFreewARobotsPointAdj(int ix, int iy); // marks the robot trace with void adjustToFreeLineNoEnds (int ixr, int iyr, int ixe, int iye);// adjust points along line to free without ends protected: //std::vector<std::vector<unsigned short int> > PP, NP, NN, PN; // for quadrants of the plains //double res; // one map grid corresponds to that much of an axis int rradius; // robot sqare in map pixels, set to 0 to disable int ssrange; // sensor range in map pixel, set to 0 to disable int horizon; // horizon around the robot that needs to be accessed ArMutex myMutex; // lock/unlock for multithreading }; inline void NAV_Map::adjustToOcupValue (unsigned short & p) { unsigned short r = maskObstacle(p); p = maskStatuses(p) | ( r + ( (getValMaxObstacle()-r)>>1 ) ); } inline void NAV_Map::adjustToFreeValue (unsigned short & p) { p = maskStatuses(p) | ( maskObstacle(p)>>2 ); } inline void NAV_Map::adjustToRobotValue (unsigned short & p) { p = p | getValRobot(); // both this robot and all robots bits } inline void NAV_Map::adjustToARobotsValue (unsigned short & p) { p = p | getValARobots(); // only all robots bit } inline void NAV_Map::adjustToOcupPoint (int ix, int iy) { adjustToOcupValue(refRawValue(ix,iy)); // adjusts the point to acupied } inline void NAV_Map::adjustToFreePoint (int ix, int iy) { adjustToFreeValue(refRawValue(ix,iy)); // adjusts the point to free } inline void NAV_Map::adjustToFreewRobotPoint (int ix, int iy) { unsigned short& c=refRawValue(ix,iy); adjustToFreeValue(c); adjustToRobotValue(c); // marks the robot trace and adjusts the point to free } inline void NAV_Map::adjustToFreewARobotsPoint (int ix, int iy) { unsigned short& c=refRawValue(ix,iy); adjustToFreeValue(c); adjustToARobotsValue(c); // marks the all robots trace and adjusts the point to free } #endif