Scan.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 __SCAN_H_
#define __SCAN_H_

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

#include <stdlib.h>
#include <string>
#include <sstream>
#include <vector>
#include <assert.h>
#include <pthread.h>

//BOOST SERIALIZATION
#ifdef FOUND_BOOST_SER
 #include <boost/serialization/vector.hpp>
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
#endif

#include <Time.h> 
#include <Spaces.h>
#include <Thread.h>

#include "ScanTypes.h"

namespace MipBaselib {
class Ray{
 private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version){
   ar & _reading;
   ar & _bearing;
   ar & _valid;
  }
#endif
  
  Decimal _reading; 
  Angle _bearing; 
  bool   _valid;   
 public:
  Ray() {
   _reading = 0.0;
   _bearing = Angle(0.0);
   _valid   = false;
  };
  Ray(Decimal r,Angle b,bool v) {
   _reading = r;
   _bearing = b;
   _valid   = v;
  };
  Ray(const Ray& r) {
   _reading = r._reading;
   _bearing = r._bearing;
   _valid   = r._valid;
  };
  Ray& operator=(const Ray& r){
   if (this != &r){
    _reading = r._reading;
    _bearing = r._bearing;
    _valid    = r._valid;
   }
   return *this;
  }
  bool operator==(const Ray &a) const {
   if( (_reading == a._reading)&&(_bearing == a._bearing)&&(_valid == a._valid) )
    return true;
   else return false;
  }
  bool operator!=(const Ray &other) const {
   return !(*this == other);
  }
  Decimal reading () {
   return _reading;
  }
  Angle bearing () {
   return _bearing;
  }
  bool valid () {
   return _valid;
  }
  void setValid (bool v) {
   _valid = v;
  }
  string print(){
   stringstream s;
   s.precision(5);
   s.setf(ios::fixed,ios::floatfield);
   if(_valid)
    s << "[" << _reading << " m, " << _bearing.print() << ", valid]" ;
   else
    s << "[" << _reading << " m, " << _bearing.print() << ", invalid]" ;
   return s.str();
  }
};

static bool _compareByBearing(Ray first, Ray second){
 return (first.bearing().dCast2Pi()<second.bearing().dCast2Pi());
}




typedef vector<Ray> Rays;


class Raymm{
 private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version){
   ar & _reading;
   ar & _bearing;
   ar & _valid;
  }
#endif

  
    UCoordmm _reading; 
    Anglem _bearing; 
    bool   _valid;   
 public:
  Raymm() {
   _reading = 0;
   _bearing = Anglem(0);
   _valid   = false;
  };
  Raymm(UCoordmm r,Anglem b,bool v) {
   _reading = r;
   _bearing = b;
   _valid   = v;
  };
  Raymm(const Raymm& r) {
   _reading = r._reading;
   _bearing = r._bearing;
   _valid   = r._valid;
  };
  Raymm& operator=(const Raymm& r){
   if (this != &r){
    _reading = r._reading;
    _bearing = r._bearing;
    _valid    = r._valid;
   }
   return *this;
  }
  bool operator==(const Raymm &a) const {
   if( (_reading == a._reading)&&(_bearing == a._bearing)&&(_valid == a._valid) )
    return true;
   else return false;
  }
  bool operator!=(const Raymm &other) const {
   return !(*this == other);
  }
  UCoordmm reading () {
   return _reading;
  }
  Anglem bearing () {
   return _bearing;
  }
  bool valid () {
   return _valid;
  }
  void setValid (bool v) {
   _valid = v;
  }
  string print(){
   stringstream s;
   if(_valid)
    s << "[" << _reading << " mm, " << _bearing.print() << ", valid]" ;
   else
    s << "[" << _reading << " mm, " << _bearing.print() << ", invalid]" ;
   return s.str();
  }
};



typedef vector<Raymm> Raymms;



class Scan{
 private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version){
   ar & _scanSize;
   ar & _rays;
   ar & _tStamp;
   ar & _valid;
  }
#endif
    
  ScanTypes _scanType;
  
  Rays  _rays; 
  Raymms  _raymms;  
  Time _tStamp; 
  bool _valid; 

  EnhancedMutEx _eMutEx; 
  
  /*static constants*/
  static const int SCAN_RESERV_SIZE = 1024; 

  int _scanSize;
  
  //Decimal _coneAmplitude; ///<The amplitude of a cone, stored for efficiency.
  Ray* _maxBearingRay; 
  Ray* _minBearingRay;

  bool _computeAngRanges(){
   if(!_eMutEx.allowed()) return false;
    _maxBearingRay =  &(*_rays.rbegin());
    _minBearingRay = &(*_rays.begin());
   return true;
  };
  
 public:
  Scan(){
   _rays.reserve(SCAN_RESERV_SIZE);
   _valid = false;
   _scanType = SCAN_SENS;
   _scanSize=0;
   _maxBearingRay=0;
   _minBearingRay=0;
  }
  Scan(ScanTypes type){
   _rays.reserve(SCAN_RESERV_SIZE);
   _valid = false;
   _scanType = type;
/*   cout << "type = " << type << endl;
   cout << "_scanType = " << _scanType << endl;*/
   _scanSize=0;
   _maxBearingRay=0;
   _minBearingRay=0;
  }
  Scan(Rays r,Time t);
  
  Scan(Raymms r,Time t);
  
  /*TODO verificare anche l'allowing su s*/
  Scan(const Scan &s);
  
  /*TODO verificare anche l'allowing su s*/
  Scan& operator=(const Scan& s){
//    cout << pthread_self() << " DEBUG = 1" << endl;
   if(!_eMutEx.allowed() ){/*if not allowed reset*/
//     cout << pthread_self()<< " DEBUG = 2" << endl;
    Scan nullScan;
    _scanType = nullScan._scanType;
    _rays     = nullScan._rays;
    _raymms   = nullScan._raymms;
    _tStamp   = nullScan._tStamp;
    _valid    = nullScan._valid;
    _scanSize = nullScan._scanSize;
    _maxBearingRay=nullScan._maxBearingRay;
    _minBearingRay=nullScan._minBearingRay;
   }
   if (this != &s){
//     cout << pthread_self() << " DEBUG = 2bis" << endl;
    _scanType = s._scanType;
    _rays     = s._rays;
    _raymms   = s._raymms;
    _tStamp   = s._tStamp;
    _valid    = s._valid;
    _scanSize = s._scanSize;
    _maxBearingRay=s._maxBearingRay;
    _minBearingRay=s._minBearingRay;
   }
   return *this;
  }
  
  bool operator==(Scan &s) {
   
   if(!s._eMutEx.allowed()) return false;
   if(!_eMutEx.allowed()) return false;
   
   if (_scanType != s._scanType) return false;
   
   int scanSize;
   if(!getSize(scanSize)) return false;
   
   if (_scanType == SCAN_SENS){
    for(int i=0;i<scanSize;i++){
      Ray r,rOther;
      if(!getRay(r,i)) return false;
      if(!s.getRay(rOther,i)) return false;
      if (r != rOther) return false;
    }
   } else if (_scanType == SCAN_SENSMM){
    for(int i=0;i<scanSize;i++){
     Raymm r,rOther;
     if(!getRay(r,i)) return false;
     if(!s.getRay(rOther,i)) return false;
     if (r != rOther) return false;
    }
   }
   return true;
  }
  
  bool operator!=(Scan &s) {
   return !(*this == s);
  }
  
  bool getConeAmplitude(Decimal &coneAmplitude);
  
  bool getScanAngle(Decimal &scanAngle);
  
  /*two mandatory wrappers*/
  bool askExclusiveAccess(Time timeout){
   return _eMutEx.askExclusiveAccess(timeout);
  }
  
  void leaveExclusiveAccess(){
   _eMutEx.leaveExclusiveAccess();
  }
  
  bool allowed(){
   return _eMutEx.allowed();
  }
  
  bool reserve(int n){
   if(!_eMutEx.allowed()){
    return false;
   } else {
    if (_scanType == SCAN_SENS){
     _rays.reserve(n);
    }else if (_scanType == SCAN_SENSMM){
     _raymms.reserve(n);
    }else {
     return false;
    }
    return true;
   }
  }
  
  /*get functions*/
  bool valid(bool& validity){
   if(!_eMutEx.allowed()) return false;
   validity = _valid;
   return true;
  }
  
  bool getScanType(ScanTypes& type){
   type = _scanType;
   return true;
  }
  
  bool getSize(int& size);

  Angle getAngRes(){
   return (_rays[1].bearing()-_rays[0].bearing());
  };
  
  bool getTimeStamp(Time& timestamp){
   if(!_eMutEx.allowed()) return false;
   timestamp = _tStamp;
   return true;
  }
  
  bool getRay(Ray& ray, int index);
  
  bool getRay(Raymm& raymm, int index);
  
  bool setRay(Ray& ray, int index);
  
  bool setRay(Raymm& raymm, int index);
  
  bool setTimeStamp(Time t){
   if(!_eMutEx.allowed()) return false;
   _tStamp = t;
   return true;
  }
  bool addRay(const Ray r){
   if (_scanType == SCAN_SENS){
    if(!_eMutEx.allowed()) return false;
    _rays.push_back(r);
    _valid  = true;
    _scanSize++;
    _computeAngRanges();
    return true;
   } else return false;
  }
  
  bool addRay(const Raymm r){
   if (_scanType == SCAN_SENSMM){
    if(!_eMutEx.allowed()) return false;
    _raymms.push_back(r);
    _valid  = true;
    _scanSize++;
    _computeAngRanges();
    return true;
   } else return false;
  }
  
  bool clear(){
   if(!_eMutEx.allowed()) return false;
   _rays.clear();
   _raymms.clear();
   _valid  = false;
   _scanSize=0;
   _maxBearingRay = 0;
   _minBearingRay = 0;
   return true;
  }
  
  bool voidReadings(Decimal minReading,Decimal maxReading);
  
  string print();
  
  void printJson(fstream& s);
  
  UCoordmm unmutexedGetReading(int index){
   if (_scanType == SCAN_SENSMM) return _raymms[index].reading();
   else return 0;
  }
  
  bool unmutexedGetValid(int index){
   if (_scanType == SCAN_SENSMM) return _raymms[index].valid();
   else return false;
  }
  
  
};

}; // end of namespace


#endif

/* @} */