Grid.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 __GRID_H_
#define __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 <Spaces.h>
#include <assert.h>
#include <math.h>
#include <Scan.h>
#include <algorithm>

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

using namespace std;

/* @{ */

enum CellPerceptionType{
    OBSTACLE_CELL,
    FRONTIER_CELL,
    LRR_CELL,
    LSR_CELL,
    UNKNOWN_CELL,
    CELL_PERC_TYPE_NUM
};

static const char* CellPerceptionTypeName[CELL_PERC_TYPE_NUM] ={
    "Obstacle Cell",
    "Frontier Cell",
    "LRR Cell",
    "LSR Cell",
    "Unknown Cell"
};


namespace MipBaselib {

class ExplorationParams{//FIXME dipendono dalle impostazioni del  sensore
    private:
        Decimal _range;                 
        Decimal _coneAmplitude; 
        Decimal _scanAngle;         
        int _samples;                       
        const char* _logPath;       
        
    public:
        ofstream _logFile;          
        
        ExplorationParams();
        
        ExplorationParams(Decimal range,Decimal scanAngle, int samples, Decimal coneAmplitude);
        
        ~ExplorationParams();
        
        ExplorationParams(const ExplorationParams& p);
        
        ExplorationParams operator=(const ExplorationParams& p);
        
        /* gets */
        
        Decimal range();
        
        Decimal coneAmplitude();
        
        Decimal scanAngle();
        
        int samples();
                
        string print();
                
        void log();
        
        /* sets */
        
        void setRange(Decimal value);
        
        void setConeAmplitude(Decimal value);
        
        void setScanAngle(Decimal value);
        
        void setSamples(int value);
                
        void setLogPath(const char* value);
};

class Cell{
    private:
        Position    _point;         
        Position    _reading;       
        DubInt      _indexes;       
        int             _vectorIndex;   
        
        Decimal _side;            
        Decimal _distVP;          
        int _distLSRBound;          
        
        unsigned int _cone;     
        unsigned int _frame;    
        
        vector<Cell*> _neighbours;  

        bool _isLRR;          
        bool _isFrontier;     
        bool _isLSR;          
        bool _isPrevExpl;     
        bool _isObstacle;     
        bool _isBoundary;     
        bool _isLRRBoundary;  
        bool _isUnknown;      
        bool _isLabelA;       
        bool _isLabelB;       
        
/*
allowed cells combinations:
        _isLRR      _isFrontier     _isLSR(CurrExplored)        isPrevExplored      _isObstacle     _isBoundary     _isLRRBoundary      _unknown
1           true                false                           true                                    false                       false                   false                   false                       false
2           true                false                           true                                    false                       false                   false                   true                        false
3           false               true                            true                                    false                       false                   true                    false                       false
4           false               true                            true                                    false                       false                   true                    false                       false
5           false               false                           true                                    false                       false                   false                   false                       false
6           false               false                           true                                    false                       true                    true                    false                       false
7           false               false                           false                                   true                        false                   false                   false                       false
8           false               false                           false                                   false                       false                   false                   false                       true
*/
        bool _integrity();       
        
    public:
        vector<int> perceptionCounter; 
        
        Cell();
        
        Cell(Decimal);
        
        ~Cell();
        
        Cell(const Cell& c);
        
        Cell& operator=(const Cell& c);
        
        /* sets */
        
        void setPoint(Position value);
        
        void setReading(Position value);
        
        void setIndexes(DubInt value);
        
        void setVectorIndex(int value);
        
        void setSide(Decimal value);
        
        void setNeighbours(vector<Cell*> value);
        
        void neighPushBack(Cell* value);
        
        void setIsLRR();
        
        void setIsFrontier();
        
        void setIsLSR();
        
        void setIsPrevExpl();
        
        void setIsObstacle();
        
        void setIsUnknown();
        
        void setIsBoundary();
        
        void setIsLRRBoundary();
        
        void setIsLabelA();
        
        void setIsLabelB();
        
        void setCone (unsigned int value);
        
        void setFrame (unsigned int value);

        void setDistVP(Decimal value);
        
        void  setDistLSRBound(int value);
        
        void unsetIsLRR();
        
        void unsetIsFrontier();
        
        void unsetIsLSR();
        
        void unsetIsPrevExpl();
        
        void unsetIsObstacle();
        
        void unsetIsUnknown();
                                
        void unsetIsBoundary();
        
        void unsetIsLRRBoundary();
        
        void unsetIsLabelA();
        
        void unsetIsLabelB();
        
        void unsetAllFlags();
        
        void copyAllFlags(Cell cell);
        
        /* gets */
        
        Position point();
        
        Position reading();
        
        DubInt indexes();
        
        int vectorIndex();
        
        Decimal side();
        
        vector<Cell*> neighbours();
        
        bool isLRR();
        
        bool isFrontier();
        
        bool isLSR();
        
        bool isPrevExpl();
        
        bool isObstacle();
        
        bool isUnknown();
        
        bool isBoundary();
        
        bool isLRRBoundary();
        
        bool isLabelA();
        
        bool isLabelB();
        
        bool integrity();
        
        unsigned int cone();
        
        unsigned int frame();
        
        Decimal distVP();
        
        int distLSRBound();
        
        void merge(Cell& otherCell);
        
        string printFlags();
};


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

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


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

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



class Frontier:public vector<FrontierArc>,MIPObject{
    private:
    public:
        Frontier():vector< FrontierArc >(){}
        
        Frontier(vector< vector<Position> > frontier);
        
        ~Frontier();
        
        string getObjectName() const {
            return "Frontier";
        }
        
        void toVectVectPosition(vector< vector< Position > >  & vectPos);
        
        vector < vector < Position > > toVectVectPosition();
};


class Grid {
    private:
        
        bool _isContiguos(DubInt cell1, DubInt cell2);
    public:
        ExplorationParams _params;          
        Scan _currScan;                                 
        Decimal _resolution;                        
        Decimal _width;                                 
        Decimal _height;                                
        Decimal _distForInvalid;                        
        unsigned int _columns;                  
        unsigned int _rows;                         
        Pose  _center;                                  
        Cell* _centralCell;                         
        Position  _LLC;                                 
        Position  _RLC;                                 
        Position  _LUC;                                 
        Position  _RUC;                                 
        
        vector<Cell> _board;                        
        
        LSR _lsr;                                               
        LSRBound _lsrBound;                         
        LRR _lrr;                                               
        LRRBound _lrrBound;                         
        Frontier _frontier;                         
        LSRBound _originalFront;                
        LSRBound _tempBound;                        
        
        Grid();
        
        Grid(ExplorationParams params, Scan currScan, Pose center);
        
        Grid(ExplorationParams params, Scan currScan, Pose center, Decimal width, Decimal height, Decimal distForInvalid = 0.3);
        
        Grid(const Grid& g);
        
        Grid operator=(const Grid& g);
        
        ~Grid();
        
        
        vector<Cell*> findFrameCells(int frame);
        
        vector<Cell*> bresenham(Cell* startCell, Cell* stopCell);
        
        void defineLSR();
        
        void defineLSRBoundary( int iMinArcSize = 3, int iMinRadialSize = 1, bool bIncludeObstacleEndPoints = false );
        
        void defineFrontier();
        
        void defineLRR();
        
        unsigned int compCol(Decimal wid, Decimal res);
        
        unsigned int compRow(Decimal hei, Decimal res);
        
        void defineBoard( int iMinArcSize = 3, int iMinRadialSize = 1, bool bIncludeObstacleEndPoints = false );
        
        void updateLSR();
        
        void updateFrontier();
        
        bool initializeScan(ExplorationParams params, Scan &currScan);
        
        /* sets */
        
        void setParams(ExplorationParams value);
        
        void setResolution(Decimal value);
        
        void setWidth(Decimal value);
        
        void setHeight(Decimal value);
        
        void setColumns(int value);
        
        void setRows(int value);
        
        void setCenter(Pose value);
        
        void setCentralCell(Cell* value);
        
        void setLLC(Position value);
        
        void setRLC(Position value);
        
        void setLUC(Position value);
        
        void setRUC(Position value);
        
        /* gets */
        
        Frontier frontier();
        
        LSRBound lsrBound();
        
        LSR lsr();
        
        LRR lrr();
        
        void getExpectedScan(Scan& expScan, Pose pose, Decimal range, unsigned int samples, Angle angWidth);
        
        ExplorationParams params();
        
        Decimal resolution();
        
        Decimal width();
        
        Decimal height();
        
        int columns();
        
        int rows();
        
        Pose center();
        
        Cell* centralCell();
        
        Position LLC();
        
        Position RLC();
        
        Position LUC();
        
        Position RUC();
        
        Cell* getCell(int index);
        
        Cell* getCell (int i, int j);
        
        Cell* getCell (DubInt indexes);
        
        Cell* getCell(Position pos);
        
        Cell* getCellOnGlobal (DubInt indexes,int minI,int minJ);
        
        Cell* getCellOnGlobal (Position pos,int minI,int minJ, Position globalCenter);
        
        string print(Cell cell);
        
        void log(Cell cell);
        
        string print();
        
        void log();
        
        Position alligne(Position pos);
        
        void addScan(Scan& scan, Pose scanPose);
};


class ProbCell : public Cell {
    
    private:
    
        Decimal _logProbOfObstacle;
    
    public:
        
        ProbCell();
        ProbCell(Decimal newProbOfObstacle);
        ~ProbCell();
    
        void setProbOfObstacle( Decimal newProbOfObstacle );
        
        Decimal getProbOfObstacle( );
        
        void setLogProbOfObstacle( Decimal newLogProbOfObstacle );
        
        Decimal getLogProbOfObstacle( );
        
        void changeLogProbOfObstacle( Decimal deltaLogProbOfObstacle );
};


class ProbGrid : public Grid {
    
    private:
        static const Decimal LOG_OCCUPIED;
        static const Decimal LOG_FREE;
    
    public:
    
        vector<ProbCell> _probBoard;
    
        ProbGrid();
    
        ProbGrid(ExplorationParams params, Scan currScan, Pose center, Decimal width, Decimal height, Decimal distForInvalid = 0.3);
        
        ~ProbGrid();
    
        void addScan(Scan& scan, Pose &scanPose);
        
        Pose pickBestScanPose( );
        
        bool isGridSettled( );
        
        void defineProbBoard( );
        
        ProbCell* getProbCell(Position pos);
        
        vector<ProbCell*> bresenham(ProbCell* startCell, ProbCell* stopCell);
};



}; // end of namespace

#endif

/* @} */