PerceptionGrid.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------




#ifndef __PERCEPTION_GRID_H_
#define __PERCEPTION_GRID_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>)
#endif

#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#include <assert.h>

#include <iostream> // std::cout
#include <utility> // std::pair

#include <fstream>
#include <iostream>
#include <vector>
#include <assert.h>
#include <math.h>

#include <Scan.h>
#include <Spaces.h>
#include <Types.h>
#include <LogTrace.h>

#include <algorithm>


#define COL_NORMAL "\033[0m"
#define COL_BLACK "\033[22;30m"
#define COL_RED "\033[22;31m"
#define COL_GREEN "\033[22;32m"
#define COL_BROWN "\033[22;33m"
#define COL_BLUE "\033[22;34m"
#define COL_MAGENTA "\033[22;35m"
#define COL_CYAN "\033[22;36m"
#define COL_GRAY "\033[22;37m"
#define COL_DARK_GRAY "\033[01;30m"
#define COL_LIGHT_RED "\033[01;31m"
#define COL_LIGHT_GREEN "\033[01;32m"
#define COL_YELLOW "\033[01;33m"
#define COL_LIGHT_BLUE "\033[01;34m"
#define COL_LIGHT_MAGENTA "\033[01;35m"
#define COL_LIGH_CYAN "\033[01;36m"
#define COL_WHITE "\033[01;37m"

using namespace std;

/* @{ */


// namespace MipBaselib{};
/*
enum PerceptionCellPerceptionType{
 OBSTACLE_PERCEPTIONCELL,
 FRONTIER_PERCEPTIONCELL,
 LRR_PERCEPTIONCELL,
 LSR_PERCEPTIONCELL,
 UNKNOWN_PERCEPTIONCELL,
 PERCEPTIONCELL_PERC_TYPE_NUM
};*/
/*
static const char* PerceptionCellPerceptionTypeName[PERCEPTIONCELL_PERC_TYPE_NUM] ={
 "Obstacle PerceptionCell",
 "Frontier PerceptionCell",
 "LRR PerceptionCell",
 "LSR PerceptionCell",
 "Unknown PerceptionCell"
};*/
enum PerceptionCellType{
 LRR_PERCEPTIONCELL,      
 FRONTIER_PERCEPTIONCELL,   
 LSR_PERCEPTIONCELL,      
 PREV_EXPLORED_PERCEPTIONCELL, 
 OBSTACLE_PERCEPTIONCELL,   
 BOUNDARY_PERCEPTIONCELL,   
 LRR_BOUNDARY_PERCEPTIONCELL, 
 UNKNOWN_PERCEPTIONCELL,    
 PERCEPTIONCELL_PERC_TYPE_NUM 
};


enum PerceptionCellBoundaryType{ //thirdState
 PERCEPTIONCELL_BOUNDARY_REACHABLE,   
 PERCEPTIONCELL_NOT_BOUNDARY_REACHABLE, 
 PERCEPTIONCELL_BOUNDARY_UNREACHABLE,  
 PERCEPTIONCELL_NOT_BOUNDARY_UNREACHABLE,
 PERCEPTIONCELL_OBSTACLE,        
 PERCEPTIONCELL_BOUNDARY_TYPE_NUM    
};

static const char* PerceptionCellBoundaryTypeName[PERCEPTIONCELL_BOUNDARY_TYPE_NUM] ={
 "Boundary reachable",
 "Not Boundary reachable",
 "Boundary unreachable",
 "Not Boundary unreachable",
 "Obstacle"
};

const Decimal ROBOT_RADIUS = 0.2;


namespace MipBaselib {

class ScanPar{
 friend class Scan;
 private:
  Decimal  _linRangeMin; 
  Decimal  _linRangeMax; 
  Decimal  _linRes;   
  
 protected:
  void setLinRangeMin(Decimal value);
  
  void setLinRangeMax(Decimal value);
  
  void setLinRes(Decimal value);
  
  
 public:
  ScanPar();
  
  ScanPar(Decimal linRangeMin,Decimal linRangeMax,Decimal linRes);
  
  ScanPar(const ScanPar &s);
  
  ScanPar& operator=(const ScanPar& s);
  
  Decimal range();
  
  Decimal linRangeMax();
  
  Decimal linRangeMin();
  
  Decimal linRes();
};


class PerceptionCellState{
 private:
  bool _wasLocal;   
  char _state;    
  bool _isLSRContour; 
  
  bool _isState(char xorMask,char orMask);
  
 public:
  PerceptionCellState();
  
  ~PerceptionCellState();
  
  PerceptionCellState(const PerceptionCellState& perceptionCellstate);
  
  PerceptionCellState& operator=(const PerceptionCellState& c);
  
  void setIsLSRContour();
  
  void setIsNotLSRContour();
  
  bool isLSRContour();
  
  void setState(bool unknown,int thirdType,bool local,bool exploredOnce);
  
  void setLocal();
   
  void setNotLocal();
  
  void setIsUnknown();
  
  void setIsFrontier();
  
  void setIsInternalNonFree();
  
  void setIsInternalFree();
  
  void setIsObstacle();
  
  bool isLocal();
  
  bool wasLocal();
  
  bool isLRR();
  
  bool isFrontier();
  
  bool isLSR();
  
  bool isPrevExpl();
  
  bool isObstacle();
  
  bool isInternalFree();
  
  bool isInternalNonFree();
  
  bool isBoundary();
  
  bool isLRRBoundary();
  
  bool isUnknown();
  
  string print();
};

class PerceptionCell{
 private:
  
  Position _center;   
  Position _reading;   
  DubInt  _indexes;   
  int    _vectorIndex; 
  
  Decimal _side;     
  
  Decimal _distVP; 

  
  int _distLSRBound; 
  
  unsigned int _cone;  
  unsigned int _frame; 
  
  vector<PerceptionCell*> _neighbours; 
  
  PerceptionCellState _state; 
  
 public:
  vector<int> perceptionCounter;
  
  inline void print();
  
  PerceptionCell();
  
  PerceptionCell(Decimal);
  
  ~PerceptionCell();
  
  PerceptionCell(const PerceptionCell& c);
  
  PerceptionCell(Decimal size,Position center,int vectorIndex, DubInt indexes,int cone, int frame,Position reading=Position(0.0,0.0));

  PerceptionCell& operator=(const PerceptionCell& c);
  
  inline void setState(bool unknown,int thirdType,bool local,bool exploredOnce);
  
  inline void setLocal();
  
  inline void setNotLocal();
  
  inline void setPoint(Position value);
  
  inline void setCenter(Position value);
  
  inline void setReading(Position value);
  
  inline void setIsFrontier();
  
  inline void setIsObstacle();
  
  inline void setIsInternalFree();
  
  inline void setIsInternalNonFree();
  
  inline void setIsUnknown();
  
  inline void setIsLSRContour();
  
  inline void setIsNotLSRContour();
  
  inline bool wasLocal();
  
  inline bool isLRR();

  inline bool isFrontier();

  inline bool isLSR();

  inline bool isPrevExpl();

  inline bool isObstacle();
  
  inline bool isInternalFree();
  
  inline bool isInternalNonFree();
  
  inline bool isUnknown();
  
  inline bool isBoundary();
  
  inline bool isLRRBoundary();
  
  inline bool isLSRContour();
  
  //*******for compatibiliy only
  void setVectorIndex(int k){}
  void setIndexes(DubInt k){}
  void unsetIsUnknown(){}
  void setIsLSR(){}
  void setIsLRR(){}
  void setIsBoundary(){}
  void setIsLRRBoundary(){}
  void unsetIsLRRBoundary(){}
  void setIsPerceptionFrontier(){}
//   void setIsObstacle(){}
  void unsetAllFlags(){}
  //****************************
  
  inline bool isPerceptionFrontier(){}
  
  inline void setNeighbours(vector<PerceptionCell*> value);
  
  inline void neighPushBack(PerceptionCell* value);
  
  inline void setCone (unsigned int value);
  
  inline void setFrame (unsigned int value);
  
  inline void  setDistLSRBound(int value);
  
  inline Position point();
  
  inline Position center();
  
  inline Position reading();
  
  inline DubInt indexes();
  
  inline int vectorIndex();
  
  inline Decimal side();
  
  inline vector<PerceptionCell*> neighbours();
  
  inline unsigned int cone();
  
  inline unsigned int frame();
  
  inline Decimal distVP();
  
  inline int distLSRBound();
  
  inline void merge(PerceptionCell& otherPerceptionCell);
  
  inline string printFlags();
};

class PerceptionFrontierArc{
 private:
  bool _isRangeEdge;    
  vector<DubInt> _vec;  
  
 public:
  PerceptionFrontierArc();
  
  PerceptionFrontierArc(const PerceptionFrontierArc& arc);
  
  PerceptionFrontierArc operator=(const PerceptionFrontierArc& arc);
  
  inline DubInt operator[](size_t index) const;
  
  inline vector<DubInt>::iterator begin();
  
  inline vector<DubInt>::iterator end();
  
  inline void push_back(DubInt cell);
  
  inline void pop_back();
  
  inline void insert(vector<DubInt>::iterator pos, DubInt value);
  
  inline void insert(vector<DubInt>::iterator pos, vector<DubInt>::iterator begin, vector<DubInt>::iterator end );
  
  inline vector<DubInt>::iterator erase (vector<DubInt>::iterator position );

  inline vector<DubInt> vec();
  
  inline int size();
  
  inline void setIsRangeEdge();
  
  inline void unsetIsRangeEdge();
  
  inline bool isRangeEdge();
  
  string print();
};


class PerceptionFrontier:public vector<PerceptionFrontierArc>,MIPObject{
 private:
 public:
  PerceptionFrontier();
  
  PerceptionFrontier(vector< vector<Position> > PerceptionFrontier);
  
  ~PerceptionFrontier();
  
  string getObjectName() const {
   return "PerceptionFrontier";
  }
};



typedef vector<DubInt> LSR;    
typedef vector<DubInt> LSRBound; 
typedef vector<DubInt> LRR;    
typedef vector<DubInt> LRRBound; 


class PerceptionGridPar:public MIPObject{
 private:
  ScanPar _scanPar;
  Pose _center;
  Decimal _cellSize;
  int _nCellX;
  int _nCellY;
 public:
  PerceptionGridPar();
  
  PerceptionGridPar(ScanPar scanPar,Pose center,Decimal cellSize,int nCellX,int nCellY);
  
  PerceptionGridPar(const PerceptionGridPar& g);
  
  PerceptionGridPar& operator=(const PerceptionGridPar& g);
  
  inline Pose center();
  
  inline Decimal cellSize();
  
  inline int nCellX();
  
  inline int nCellY();
  
  inline Decimal width();
  
  inline Decimal height();
  
  bool setSize(Decimal height, Decimal width, Decimal cellSize);
  
  bool setSize(Decimal width, int nCellX, int nCellY);
  
  string getObjectName() const {
   return "PerceptionGridPar";
  }
  
  inline ScanPar* scanPar();
};

class PerceptionGrid:public MIPObject {
 private:
  PerceptionGridPar _perceptionGridPar;
  
  Scan _currScan;                

  PerceptionCell* _centralCell;            

  vector<PerceptionCell> _board;           

  LSR _lsr;                      
  LSRBound _lsrBound;            
  LRR _lrr;                      
  LRRBound _lrrBound;            
  PerceptionFrontier _PerceptionFrontier;            
  LSRBound _originalFront;       
  LSRBound _tempBound;           
 
  ScanPar* _scanPar(){
   return _perceptionGridPar.scanPar();
  };
   

  
  bool _areContiguous(DubInt cell1, DubInt cell2);
  
  bool _areSufficientlyClose(PerceptionCell*  cellPointerA,PerceptionCell*  cellPointerB,Decimal distance);
 
  
  bool _isOnBoard(int vectorIndex){
   if ((vectorIndex <0) || (vectorIndex > width()*height()-1)){
    return false;
   }else{
    return true;
   }
  }
  
  bool _isOnBoard(DubInt indexes){
   int i=indexes.i();
   int j=indexes.j();
   if (i<1 || j<1 || i>rows() || j>columns()){
    return false;
   }else{
   return true;
   }
  }
  
  DubInt _getIndexesFromVectorIndex(int vectorIndex){
   int numcolumns = columns();
   int i=(vectorIndex/numcolumns)+1;
   int j=(vectorIndex+1)-(numcolumns*(i-1));
   return DubInt(i,j);
  }
  

  
//   bool _isLSRBoundary(DubInt indexes){
//    vector<DubInt>::iterator it;
//    for (it=_lsrBound.begin();it!=_lsrBound.end();it++){
//     if ((*it)==indexes){
//      return true;
//     }
//    }
//    return false;
//   }
  
  bool _isLSRBoundary(DubInt indexes){
//    return (getCell(indexes)->isFrontier() || getCell(indexes)->isObstacle());
   return getCell(indexes)->isLSRContour();
  }
  
  bool _isLSR(DubInt indexes){
   return (getCell(indexes)->isInternalFree() || getCell(indexes)->isInternalNonFree());
  }
  

 
 void _setIsLSR(DubInt cellIndexes);

 void _colorLine(int leftx,int rightx,int y,void (PerceptionGrid::*colorCell)(DubInt cellIndexes,Decimal dist));
//  {
//   for (int x = leftx;x<=rightx;x++){
//    getCell(x,y)->setIsLSR();
//    _lsr.push_back(DubInt(x,y));
//   }
//  }
//  
 void _colorLine(int leftx,int rightx,int y);

//  void _scanLineFill(int xx,int yy);
 
 void _scanLineFill(int xx,int yy,bool (PerceptionGrid::*stopTest)(DubInt cellIndexes),
           bool (PerceptionGrid::*alreadyColoredTest)(DubInt cellIndexes),void (PerceptionGrid::*colorCell)(DubInt cellIndexes,Decimal dist));
 
 void _classifyNonObstacleCell(PerceptionCell * perceptionCell);
 
 void _eraseInternalCellsOnLSRContour();

 void _classifyInternalCell(DubInt cellIndexes,Decimal robotRadius){
  int squarewidth = (int)(robotRadius/(Decimal)(perceptionGridPar()->cellSize()))+1;

  int i = cellIndexes.i();
  int j = cellIndexes.j();

  int starti = (i-squarewidth/2);
  int endi = (i+squarewidth/2); 
  int startj = (j-squarewidth/2);
  int endj = (j+ squarewidth/2);
//   cout << "start i "<< starti<<" endi " << endi<<endl;
//   cout << "start j" << startj<<" endj " << endj<<endl;
  starti = (starti >= 0)?starti:0;
  startj = (startj >= 0)?startj:0;
  endi = (endi >= (rows() -1))?(rows()-1):endi;
  endj = (endj >= (columns() -1))?(columns()-1):endj;

//   cout << "start i "<< starti<<" endi " << endi<<endl;
//   cout << "start j" << startj<<" endj " << endj<<endl;
  PerceptionCell * ptr1 = getCell(cellIndexes);
  PerceptionCell * ptr2 = NULL;
  bool escape = false;
  bool test = true;
  
  for (int ci=starti;ci<=endi;ci++){
   for (int cj=startj;cj<=endj;cj++){
    ptr2 = getCell(ci,cj);
    //no distinction for a previously local cell and a non one if it is stricly inside the LRR
    if (ptr1->wasLocal()){
//      test = ptr2->isObstacle();
     test = (ptr2->isObstacle()|| ptr2->isFrontier());
    }else{
     test = (ptr2->isObstacle()|| ptr2->isFrontier());
    }
    if (test){
     if (_areSufficientlyClose(ptr1,ptr2,robotRadius)){
      escape = true;
      break;
     }
    }
   }
   if (escape) break;
  }
  ptr1->setLocal();
  if (escape){
//    cout << "set is non free"<<endl;
   ptr1->setIsInternalNonFree();
  }else{
//    cout << "set is free"<<endl;
   ptr1->setIsInternalFree();
  }
 }
  
 void _findInternalCellsOnLSRContour();
 
 public:
  void _setAllNotLocal();
  
  PerceptionGridPar* perceptionGridPar(){
   return &_perceptionGridPar;
  }
  void _clear(){
   vector<PerceptionCell>::iterator it;
   for (it=_board.begin();it!=_board.end();it++){
    (*it).setIsUnknown();
    (*it).setIsNotLSRContour();
   }
  };
  void _scanLineFill(int xx,int yy);
  
  void _computeLSR(Pose pose);
  
  bool _isUnknown(DubInt indexes);
  
  void _setUnknown(DubInt indexes,Decimal useless = 0);
  
  void _eraseLocalRegion(Pose pose);
  
  string getObjectName() const {
   return "PerceptionGrid";
  }
   
  PerceptionGrid(PerceptionGridPar perceptionGridPar,Scan currScan);
  
  PerceptionGrid(const PerceptionGrid& g);
  
  PerceptionGrid operator=(const PerceptionGrid& g);
  
  ~PerceptionGrid();
  
  
  vector<PerceptionCell*> findFrameCells(int frame);
  
  vector<PerceptionCell*> bresenham(PerceptionCell* startCell, PerceptionCell* stopCell);
  
  void defineLSR();
  
  void defineLSRBoundary( int iMinArcSize = 3, int iMinRadialSize = 1, bool bIncludeObstacleEndPoints = false );
  
  vector<DubInt> computeLSRContour(Scan & scan, int iMinArcSize=3, int iMinRadialSize=1, bool bIncludeObstacleEndPoints=false);
  
  
  void _eraseLSRContour();

  vector<DubInt> computeLSRContour(Scan & scan, Pose pose, Decimal maxDistance);
  
  void _addRobotShape(vector<DubInt> * lsrContour,PerceptionFrontierArc * currentArc,Pose pose);

  
  void definePerceptionFrontier();
  
  void defineLRR();
  
  void defineBoard( int iMinArcSize = 3, int iMinRadialSize = 1, bool bIncludeObstacleEndPoints = false );
  
  void updateLSR();
    
   void updatePerceptionFrontier();
 
   bool initializeScan(ScanPar params, Scan &currScan);

//     gets
  
  PerceptionFrontier Frontier();
  
  LSRBound lsrBound();
  
  LSR lsr();
  
  LRR lrr();
  
  void getExpectedScan(Scan& expScan, Pose pose, Decimal range, unsigned int samples, Angle angWidth);
  
  Decimal cellSize();
  
  Decimal width();
  
  Decimal height();
  
  int columns();
  
  int rows();
  
  Pose center();
  
  PerceptionCell* centralCell();
  
  Position LLC();
  
  Position RLC();
  
  Position LUC();
  
  Position RUC();
  
  PerceptionCell* getCell(int index);
  
  PerceptionCell* operator[](int index);

  PerceptionCell* operator()(int i,int j);
  
  PerceptionCell* getCell (int i, int j);
  
  PerceptionCell* getCell (DubInt indexes);
  
  PerceptionCell* getCell(Position pos);
  
  PerceptionCell* getCellOnGlobal (DubInt indexes,int minI,int minJ);

  PerceptionCell* getCellOnGlobal (Position pos,int minI,int minJ, Position globalCenter);
  
  Position alligne(Position pos);
  
//   void merge(PerceptionGrid& otherPerceptionGrid, Pose changeOfCoord);
  
  void addScan2(Scan& scan, Pose scanPose);
  
  void frontierToVectVectPosition(vector< vector< Position > >& vectPos, PerceptionFrontier& frontiere);
  
  vector < vector<Position> > frontierToVectVectPosition(PerceptionFrontier& frontiere);

  void addScan(Scan & scan,Pose & pose);
};


}; // end of namespace

/* @} */

#endif