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

#include "nav_map.h" #include <fstream> #include <cstring> #include <cstdlib> #include <cassert> NAV_Map::NAV_Map(double resol, double robotradius, double sensorrange) : myMutex(), NAV_Map_Base( resol, max(robotradius,sensorrange)+100*resol, getValUnknown()), rradius(robotradius/res), ssrange(sensorrange/res), horizon( max(robotradius ,sensorrange)/res+10 ) { clear(); } void NAV_Map::clear() { minx=resizeravval; maxx=resizeravval; miny=resizeravval; maxy=resizeravval; PP.clear(); PP.resize(maxx); for (int i=0; i<maxx; i++) PP[i].resize(maxy, getValUnknown()); NP.clear(); NP.resize(minx); for (int i=0; i<minx; i++) NP[i].resize(maxy, getValUnknown()); NN.clear(); NN.resize(minx); for (int i=0; i<minx; i++) NN[i].resize(miny, getValUnknown()); PN.clear(); PN.resize(maxx); for (int i=0; i<maxx; i++) PN[i].resize(miny, getValUnknown()); } void NAV_Map::addObstacle(const ArPose &point) { int ix= static_cast<int>(point.getX()/res); int iy= static_cast<int>(point.getY()/res); resizeRaw(ix,iy); adjustToOcupPoint(ix,iy); } void NAV_Map::addFreePoint(const ArPose &point) { int ix= static_cast<int>(point.getX()/res); int iy= static_cast<int>(point.getY()/res); resizeRaw(ix,iy); adjustToFreePoint(ix,iy); } void NAV_Map::addFreePointR(const ArPose &point) { int ix= static_cast<int>(point.getX()/res); int iy= static_cast<int>(point.getY()/res); resizeRaw(ix,iy); adjustToFreewRobotPointAdj(ix,iy); } void NAV_Map::addFreePointAR(const ArPose &point) { int ix= static_cast<int>(point.getX()/res); int iy= static_cast<int>(point.getY()/res); resizeRaw(ix,iy); adjustToFreewARobotsPointAdj(ix,iy); } unsigned short NAV_Map::getPoint (double x, double y) { int ix= static_cast<int>(x/res); int iy= static_cast<int>(y/res); unsigned short val; if ( ix<=-minx||ix>=maxx||iy<=-miny||iy>=maxy) val=getValUnknown(); // point outside of the map - unknown else val=getRawValue(ix,iy); return(val); } void NAV_Map::addLineToObst(const ArPose &echo, const ArPose &from) { int ixe= static_cast<int>(echo.getX()/res); int iye= static_cast<int>(echo.getY()/res); int ixr= static_cast<int>(from.getX()/res); int iyr= static_cast<int>(from.getY()/res); resizeRaw(ixe,iye); resizeRaw(ixr,iyr); adjustToFreePoint(ixr,iyr); adjustToFreeLineNoEnds(ixr,iyr,ixe,iye); adjustToOcupPoint(ixe,iye); } void NAV_Map::addLineToObstR(const ArPose &echo, const ArPose &robot) { int ixe= static_cast<int>(echo.getX()/res); int iye= static_cast<int>(echo.getY()/res); int ixr= static_cast<int>(robot.getX()/res); int iyr= static_cast<int>(robot.getY()/res); resizeRaw(ixe,iye); resizeRaw(ixr,iyr); adjustToFreewRobotPointAdj(ixr,iyr); adjustToFreeLineNoEnds(ixr,iyr,ixe,iye); adjustToOcupPoint(ixe,iye); } void NAV_Map::addLineToObstAR(const ArPose &echo, const ArPose &robot) { int ixe= static_cast<int>(echo.getX()/res); int iye= static_cast<int>(echo.getY()/res); int ixr= static_cast<int>(robot.getX()/res); int iyr= static_cast<int>(robot.getY()/res); resizeRaw(ixe,iye); resizeRaw(ixr,iyr); adjustToFreewARobotsPointAdj(ixr,iyr); adjustToFreeLineNoEnds(ixr,iyr,ixe,iye); adjustToOcupPoint(ixe,iye); } void NAV_Map::addLineToFree(const ArPose &none, const ArPose &from) { int ixe= static_cast<int>(none.getX()/res); int iye= static_cast<int>(none.getY()/res); int ixr= static_cast<int>(from.getX()/res); int iyr= static_cast<int>(from.getY()/res); resizeRaw(ixe,iye); resizeRaw(ixr,iyr); adjustToFreePoint(ixr,iyr); adjustToFreeLineNoEnds(ixr,iyr,ixe,iye); adjustToFreePoint(ixe,iye); } void NAV_Map::addLineToFreeR(const ArPose &none, const ArPose &from) { int ixe= static_cast<int>(none.getX()/res); int iye= static_cast<int>(none.getY()/res); int ixr= static_cast<int>(from.getX()/res); int iyr= static_cast<int>(from.getY()/res); resizeRaw(ixe,iye); resizeRaw(ixr,iyr); adjustToFreewRobotPointAdj(ixr,iyr); adjustToFreeLineNoEnds(ixr,iyr,ixe,iye); adjustToFreePoint(ixe,iye); } void NAV_Map::addLineToFreeAR(const ArPose &none, const ArPose &from) { int ixe= static_cast<int>(none.getX()/res); int iye= static_cast<int>(none.getY()/res); int ixr= static_cast<int>(from.getX()/res); int iyr= static_cast<int>(from.getY()/res); resizeRaw(ixe,iye); resizeRaw(ixr,iyr); adjustToFreewARobotsPointAdj(ixr,iyr); adjustToFreeLineNoEnds(ixr,iyr,ixe,iye); adjustToFreePoint(ixe,iye); } void NAV_Map::setCustoms(double x, double y, unsigned char s) { int ix= static_cast<int>(x/res); int iy= static_cast<int>(y/res); resizeRaw(ix,iy); unsigned short& c=refRawValue(ix,iy); c = (c&0xE0FF) | ((s&0x1F)<<8); } void NAV_Map::andCustoms(double x, double y, unsigned char s) { int ix= static_cast<int>(x/res); int iy= static_cast<int>(y/res); resizeRaw(ix,iy); unsigned short& c=refRawValue(ix,iy); c = (c&0xE0FF) | ( (c&0x1F00) & ((s&0x1F)<<8) ); } void NAV_Map::orrCustoms(double x, double y, unsigned char s) { int ix= static_cast<int>(x/res); int iy= static_cast<int>(y/res); resizeRaw(ix,iy); unsigned short& c=refRawValue(ix,iy); c = c | ((s&0x1F)<<8); // c = (c&0xE0FF) | ( (c&0x1F00) | ((s&0x1F)<<8) ); } void NAV_Map::xorCustoms(double x, double y, unsigned char s) { int ix= static_cast<int>(x/res); int iy= static_cast<int>(y/res); resizeRaw(ix,iy); unsigned short& c=refRawValue(ix,iy); c = (c&0xE0FF) | ( (c&0x1F00) ^ ((s&0x1F)<<8) ); } void NAV_Map::resizeRaw(int ix, int iy) { bool needtoresize=false; if (ix>=0) { if (ix+horizon>=maxx) { maxx=ix+resizeravval; needtoresize=true; } } else { if (-ix+horizon>=minx) { minx=-ix+resizeravval; needtoresize=true; } } if (iy>=0) { if (iy+horizon>=maxy) { maxy=iy+resizeravval; needtoresize=true; } } else { if (-iy+horizon>=miny) { miny=-iy+resizeravval; needtoresize=true; } } if (needtoresize) { PP.resize(maxx); for (int i=0; i<maxx; i++) PP[i].resize(maxy, getValUnknown()); NP.resize(minx); for (int i=0; i<minx; i++) NP[i].resize(maxy, getValUnknown()); NN.resize(minx); for (int i=0; i<minx; i++) NN[i].resize(miny, getValUnknown()); PN.resize(maxx); for (int i=0; i<maxx; i++) PN[i].resize(miny, getValUnknown()); } } void NAV_Map::adjustToFreeLineNoEnds(int xr,int yr,int xe,int ye) { if ( xr==xe && yr==ye ) return; // its's a point, not a line! Discovered by Mathew Draear alumni of 2013 // Bresenham algorithm for drawing a line in graphics int xl = xr; // x on line of sight int yl = yr; // y on line of sight int dy = ye - yr; int dx = xe - xr; int stepx, stepy; if (dy < 0) { dy = -dy; stepy = -1; } else { stepy = 1; } if (dx < 0) { dx = -dx; stepx = -1; } else { stepx = 1; } dy <<= 1; // dy is now 2*dy dx <<= 1; // dx is now 2*dx if (dx > dy) { int fraction = dy - (dx >> 1); // same as 2*dy - dx while(1) { if (fraction >= 0) { yl += stepy; fraction -= dx; // same as fraction -= 2*dx } xl += stepx; fraction += dy; // same as fraction -= 2*dy if (xl == xe) break; // leave just before reaching end adjustToFreePoint(xl, yl); } } else { int fraction = dx - (dy >> 1); while(1) { if (fraction >= 0) { xl += stepx; fraction -= dy; } yl += stepy; fraction += dx; if (yl == ye) break; // leave just before reaching end adjustToFreePoint(xl, yl); } } } /* This modification was proposed by John Hesling in April 2008 The purpose is to make thicker marks of the robot presence so that it is easier to detect in case we return to about the same spot. Originally the rob was leaving only relatively dense dotted trail with density depending on its velocity */ void NAV_Map::adjustToFreewRobotPointAdj(int ix, int iy) { if (ssrange>0) { int xsleft = ix-ssrange; int xsright = ix+ssrange; int ysleft = iy-ssrange; int ysright = iy+ssrange; int xrleft = ix-rradius-1; int xrright = ix+rradius+1; int yrleft = iy-rradius-1; int yrright = iy+rradius+1; resizeRaw(xsleft,ysleft); resizeRaw(xrright,yrright); bool inrobot=true; for (int xt=xrleft; xt>=xsleft; xt--) { unsigned short v=getRawValue(xt,iy); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int xm=xrleft; xm>xt; xm--) adjustToFreePoint(xm,iy); break; } } inrobot=true; for (int xt=xrright; xt<=xsright; xt++) { unsigned short v=getRawValue(xt,iy); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int xm=xrright; xm<xt; xm++) adjustToFreePoint(xm,iy); break; } } inrobot=true; for (int yt=yrleft; yt>=ysleft; yt--) { unsigned short v=getRawValue(ix,yt); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int ym=yrleft; ym>yt; ym--) adjustToFreePoint(ix,ym); break; } } inrobot=true; for (int yt=yrright; yt<=ysright; yt++) { unsigned short v=getRawValue(ix,yt); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int ym=yrright; ym<yt; ym++) adjustToFreePoint(ix,ym); break; } } } if (rradius>0) { int xrleft = ix-rradius; int xrright = ix+rradius; int yrleft = iy-rradius; int yrright = iy+rradius; for (int xr=xrleft; xr<=xrright; xr++) for (int yr=yrleft; yr<=yrright; yr++) adjustToFreewRobotPoint(xr,yr); } else { adjustToFreewRobotPoint(ix,iy); } } void NAV_Map::adjustToFreewARobotsPointAdj(int ix, int iy) { if (ssrange>0) { int xsleft = ix-ssrange; int xsright = ix+ssrange; int ysleft = iy-ssrange; int ysright = iy+ssrange; int xrleft = ix-rradius-1; int xrright = ix+rradius+1; int yrleft = iy-rradius-1; int yrright = iy+rradius+1; bool inrobot=true; for (int xt=xrleft; xt>=xsleft; xt--) { unsigned short v=getRawValue(xt,iy); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int xm=xrleft; xm>xt; xm--) adjustToFreePoint(xm,iy); break; } } inrobot=true; for (int xt=xrright; xt<=xsright; xt++) { unsigned short v=getRawValue(xt,iy); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int xm=xrright; xm<xt; xm++) adjustToFreePoint(xm,iy); break; } } inrobot=true; for (int yt=yrleft; yt>=ysleft; yt--) { unsigned short v=getRawValue(ix,yt); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int ym=yrleft; ym>yt; ym--) adjustToFreePoint(ix,ym); break; } } inrobot=true; for (int yt=yrright; yt<=ysright; yt++) { unsigned short v=getRawValue(ix,yt); if (maskObstacle(v)!=0) break; if (maskARobots(v)==0) inrobot=false; if (!inrobot && maskARobots(v)!=0) { for (int ym=yrright; ym<yt; ym++) adjustToFreePoint(ix,ym); break; } } } if (rradius>0) { int xrleft = ix-rradius; int xrright = ix+rradius; int yrleft = iy-rradius; int yrright = iy+rradius; for (int xr=xrleft; xr<=xrright; xr++) for (int yr=yrleft; yr<=yrright; yr++) adjustToFreewARobotsPoint(xr,yr); } else { adjustToFreewARobotsPoint(ix,iy); } } bool NAV_Map::write(const char* fname, unsigned char mask, bool ascii) { mask = (mask & 0x7F); // enforce valid ranges for masking information // open the file to write to std::ofstream OU; OU.open(fname, std::ios_base::binary); if (OU.fail()) return(false); const int sizx= minx + maxx - 1; const int sizy= miny + maxy - 1; const int maxc= getValMaxObstacle(); const int filc= maxc >> 2; if (ascii) { OU << "P3\n# processed by Simplified BU Autonomnous Mobile Agents Mapping Program\n"; } else { OU << "P6\n# processed by Simplified BU Autonomnous Mobile Agents Mapping Program\n"; } OU << "# Data: composit map\n"; OU << "# Resol: res=" << res << "\n"; OU << "# Sizes: minx=" << minx << " maxx=" << maxx << " miny=" << miny << " maxy=" << maxy << "\n"; OU << "# Robot: rradius=" << rradius << " ssrange=" << ssrange << "\n"; OU << sizy << " " << sizx << " " << maxc << std::endl; if (OU.fail()) return(false); unsigned short raw; unsigned short r, g, b; unsigned char bytes[3]; assert(maxc<256); for (int x=maxx-1; x>=0; x--) { for (int y=maxy-1; y>=0; y--) { raw = PP[x][y]; r = maskObstacle(raw); if (!maskUnknown(raw)) g = filc; else g = 0; b = (maskStatuses(raw)>>8) & mask; if (ascii) { OU << r <<' '<< g <<' '<< b << std::endl; } else { bytes[0]=r; bytes[1]=g; bytes[2]=b; OU.write(reinterpret_cast<char*>(bytes),3); } } for (int y=1; y<miny; y++) { raw = PN[x][y]; r = maskObstacle(raw); if (!maskUnknown(raw)) g = filc; else g = 0; b = (maskStatuses(raw)>>8) & mask; if (ascii) { OU << r <<' '<< g <<' '<< b << std::endl; } else { bytes[0]=r; bytes[1]=g; bytes[2]=b; OU.write(reinterpret_cast<char*>(bytes),3); } } } for (int x=1; x<minx; x++) { for (int y=maxy-1; y>=0; y--) { raw = NP[x][y]; r = maskObstacle(raw); if (!maskUnknown(raw)) g = filc; else g = 0; b = (maskStatuses(raw)>>8) & mask; if (ascii) { OU << r <<' '<< g <<' '<< b << std::endl; } else { bytes[0]=r; bytes[1]=g; bytes[2]=b; OU.write(reinterpret_cast<char*>(bytes),3); } } for (int y=1; y<miny; y++) { raw = NN[x][y]; r = maskObstacle(raw); if (!maskUnknown(raw)) g = filc; else g = 0; b = (maskStatuses(raw)>>8) & mask; if (ascii) { OU << r <<' '<< g <<' '<< b << std::endl; } else { bytes[0]=r; bytes[1]=g; bytes[2]=b; OU.write(reinterpret_cast<char*>(bytes),3); } } } OU.close(); if (OU.fail()) return(false); else return(true); } bool NAV_Map::read(const char* fname, unsigned char mask) { mask = (mask & 0x7F); // enforce valid ranges for masking information // open the file to write to std::ifstream INP; INP.open(fname, std::ios_base::binary); if (INP.fail()) return(false); // check for file type char s1, s2; INP.get(s1); INP.get(s2); if (s1!='P' || (s2!='3' && s2!='6')) { return(false); } INP.ignore(256,'\n'); while (INP.peek()=='#') { char buffer[512]; INP.getline(buffer,sizeof(buffer),'\n'); char *p; p=strstr(buffer, "res="); if (p!=0) res=atof(p+4); p=strstr(buffer, "minx="); if (p!=0) minx=atoi(p+5); p=strstr(buffer, "maxx="); if (p!=0) maxx=atoi(p+5); p=strstr(buffer, "miny="); if (p!=0) miny=atoi(p+5); p=strstr(buffer, "maxy="); if (p!=0) maxy=atoi(p+5); p=strstr(buffer, "rradius="); if (p!=0) rradius=atof(p+8); p=strstr(buffer, "ssrange="); if (p!=0) ssrange=atof(p+8); } horizon=max(rradius,ssrange)/res+10; resizeravval=100+horizon; int sizx; int sizy; int maxc; INP >> sizy >> sizx >> maxc; if (INP.fail()||(sizx!=minx+maxx-1)||(sizy!=miny+maxy-1)||(maxc!=getValMaxObstacle())) return(false); if (s2=='6') INP.ignore(1); // ignore the end of line PP.resize(maxx); for (int i=0; i<maxx; i++) PP[i].resize(maxy, getValUnknown()); NP.resize(minx); for (int i=0; i<minx; i++) NP[i].resize(maxy, getValUnknown()); NN.resize(minx); for (int i=0; i<minx; i++) NN[i].resize(miny, getValUnknown()); PN.resize(maxx); for (int i=0; i<maxx; i++) PN[i].resize(miny, getValUnknown()); if (s2=='3') { unsigned short r, g, b; unsigned short raw; for (int x=maxx-1; x>=0; x--) { for (int y=maxy-1; y>=0; y--) { INP >> r >> g >> b; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); PP[x][y] = raw; } for (int y=1; y<miny; y++) { INP >> r >> g >> b; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); PN[x][y] = raw; } if (INP.fail()) break; } for (int x=1; x<minx; x++) { for (int y=maxy-1; y>=0; y--) { INP >> r >> g >> b; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); NP[x][y] = raw; } for (int y=1; y<miny; y++) { INP >> r >> g >> b; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); NN[x][y] = raw; } if (INP.fail()) break; } } else { unsigned char bytes[3]; unsigned short r, g, b; unsigned short raw; for (int x=maxx-1; x>=0; x--) { for (int y=maxy-1; y>=0; y--) { INP.read(reinterpret_cast<char*>(bytes),3); r = bytes[0]; g = bytes[1]; b = bytes[2]; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); PP[x][y] = raw; } for (int y=1; y<miny; y++) { INP.read(reinterpret_cast<char*>(bytes),3); r = bytes[0]; g = bytes[1]; b = bytes[2]; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); PN[x][y] = raw; } if (INP.fail()) break; } for (int x=1; x<minx; x++) { for (int y=maxy-1; y>=0; y--) { INP.read(reinterpret_cast<char*>(bytes),3); r = bytes[0]; g = bytes[1]; b = bytes[2]; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); NP[x][y] = raw; } for (int y=1; y<miny; y++) { INP.read(reinterpret_cast<char*>(bytes),3); r = bytes[0]; g = bytes[1]; b = bytes[2]; raw = maskObstacle(r) + maskStatuses((b&mask)<<8); if (!g) raw |= getValUnknown(); NN[x][y] = raw; } if (INP.fail()) break; } } INP.close(); if (INP.fail()) return(false); else return(true); }

//* Review of binary operations, see also (EE101 or EE311) and EE365 *

/* c = a & b - perform AND on integers a and b bit by bit c = a | b - perform OR on integers a and b bit by bit c = a ^ b - perform XOR on integers a and b bit by bit c = ~a - perform NOT on integer a bit by bit c = !a - perform boolean NOT, i.e. 0 if a!=0, 1 if a==0 c = a<<3 - perform shift left three times on integer a bit by bit, insert 0s at the right should use it instead of a*8 (8==2^3) in high performance code (that must run quickly) c = a>>3 - perform shift right three times on integer a bit by bit, insert 0s at the left should use it instead of a/8 (8==2^3) in high performance code (that must run quickly) */