Deploy.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 __DEPLOY_H_
#define __DEPLOY_H_


#include <math.h>

#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <CommModule.h>
#include <DraWin.h>
#include <ProtrusionExtr.h>
#include <Voronoi.h>
#include <CommonOptions.h>
#include <Serialization.h>

#include <CGAL/IO/Geomview_stream.h>
#include <CGAL/IO/Triangulation_geomview_ostream_2.h>

#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Polygon_set_2.h>
#include <CGAL/Boolean_set_operations_2.h>



using namespace MipAlgorithms;
using namespace MipResources;


namespace MipTasks{
 
 /* @{ */
 
 typedef CGAL::Polygon_with_holes_2<K>         Polygon_with_holes_2;
 typedef CGAL::Polygon_set_2<K>                Polygon_set_2;
 
 
 class DeployOptions : public Options {
  public:
   
   IntOption*   deployAlgo;
   IntOption*   deployCommOn;
   DecimalOption* deployCommRange;
   DecimalOption* deployEnvMaxDim;
   IntOption*   deployFillScan;
   IntOption*   deployComputeCoverage;
   
   DeployOptions(){
    /*default values*/
    deployAlgo = createIntOption(string("deployAlgo"),string("0 centroid (default), 1 weighted normal, 2 circumcenter  "),0);
    deployCommOn = createIntOption(string("deployCommOn"),string("0 communication disabled, 1 communication enabled"),1);
    deployCommRange = createDecimalOption(string("deployCommRange"),string("communication range (meters) [default: not limited]"),0.0);
    deployEnvMaxDim = createDecimalOption(string("deployEnvMaxDim"),string("environment max dimension (meters)"),4.0);
    deployFillScan = createIntOption(string("deployFillScan"),string("0 scan filling disabled, 1 scan filling enabled"),1);
    deployComputeCoverage = createIntOption(string("deployComputeCoverage"),string("0 coverage computing disabled, 1 coverage computing enabled"),0);
    
    updateValues();
   
   }
   
   string getObjectName() const{
     return "DeployOptions";
   }
 };
 
 
 enum DeployState{
  DEPLOY_FIRST_RUN,
  DEPLOY_SETUP_RUN,
  DEPLOY_NORMAL_RUN,
  DEPLOY_NUM_RUNS
 };
 
 enum DeployAlgo{
  CENTROID_ALGO=0, //does not limit the laser range
  WEIGHTED_NORMAL_ALGO=1,
  CIRCUMCENTER_ALGO=2,
  DEPLOY_NUM_ALGO
 };
 
 typedef vector<Position> ScanPoints;
 
 
 class DeployTask : public Task{
  private:
   
   static const TaskPlate _plate = DPL_TASK;
   static const long int _maxDurSec  = 0, _maxDurUsec = 700000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 250000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 100000;
   
   static const long int DEPLOY_GV_ON_DEFAULT = 0;
   static const long int DEPLOY_DW_ON_DEFAULT = 1;
   static const long int TRACE_ON_DEFAULT = 1;
   
   static const int OTHER_POS_RESERV_SIZE = 10;
   static const int SETUP_RUNS = 100;
   
   MobileRob* _mobileRob;
   CommModule* _commModule;
   
   DraWin*   _draWin;
   unsigned int _drawList;
   bool dwOn;
   
   CGAL::Geomview_stream* gv;
   bool gvOn;
   
   bool traceOn;
   
   DeployState state;
   
   DeployAlgo algo;
   
   Voronoi voronoi;
   
   Scan scan;
   
   int robId;
   Position robPosition;
   Pose robPose;
   Pose lastRobPose;
   ScanPoints robScanPoints;
   Decimal robTraveledDistance;
   
   Decimal goalReachedEps;
   int goalReachedCnt;
   int goalReachedThre;
   
   Position target;
   Position targetAbs;
   
   Decimal radius;
     
   bool commOn;
   Decimal commRange;
   Decimal envMaxDim;
   
   DeployOptions options;
   
   PosiFeatures otherRobotPositions;
   map<int,Pose> otherRobotPoses;
   map<int,Pose>::iterator otherRobotPosesIt;
   map<int,Decimal> otherRobotTraveledDistance;
   map<int,Decimal>::iterator otherRobotTraveledDistanceIt;
   map<int,ScanPoints> otherRobotScanPoints;
   map<int,ScanPoints>::iterator otherRobotScanPointsIt;
   vector<Polygon_with_holes_2> coveragePolygons;
   vector<Polygon_with_holes_2>::const_iterator coveragePolygonsIt;
   
   Decimal traveledDistance;
   Decimal coverageArea;
   
   Decimal limitedRange;
   bool limitedRangeOn;
   bool useSectors;
   
     
   void firstRun();
   void normalRun();
   void getCommPackets();
   void sendPoseAndScan();
   bool getPoseAndScan();
   void getOtherRobPosi();
   void computeVoronoiRegion();
   void chooseTarget();
   void computeTraveledDistance();
   void computeCoverage();
   Decimal polygonWithHolesArea(const Polygon_with_holes_2 & pwh);
   void computeCoverageArea();
   
   void drawPolygon(const Polygon_2 & p, Color c, int sampling=1);
   void drawPolygonWithHoles(const Polygon_with_holes_2 & pwh, Color c, int sampling=1);
   void drawCoverage(Color c, int sampling=1);
   void draw();
   
  public:
   DeployTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~DeployTask(){
   }
 
   TaskPlate getPlate() const {
    return _plate;
   }  
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
 
   TaskOutputs run(){
    static int setupRunCnt = 0;
 
    switch(state){
     case DEPLOY_FIRST_RUN:
      firstRun();
      state = DEPLOY_SETUP_RUN;
      break;
     case DEPLOY_SETUP_RUN: {
      setupRunCnt++;
      if (setupRunCnt>SETUP_RUNS) state = DEPLOY_NORMAL_RUN;
      break;
     }
     case DEPLOY_NORMAL_RUN:
      normalRun();
      break;
    }
    return TASK_OUT_GO_ON;
   }
 
 };
 
 /* @} */
 
};//end namespace MipTasks

#endif