CentralizedPHDFilterTask.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 __CPHD_FILTER_TASK_H_
#define __CPHD_FILTER_TASK_H_

#include <armadillo>

#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <ProtrusionExtr.h>
#include <GMPHDUnicycle2DIdCentrFilter.h>
#include <SharedMem.h>
#include <CommonOptions.h>
#ifndef MIP_PLATFORM_ARM
 #include <DraWin.h>
#endif
#include <CommModule.h>

#include <EKFilterUnicycle2D.h>



using namespace MipAlgorithms;
using namespace MipResources;
using namespace arma;


namespace MipTasks{
 
 /* @{ */
 
 class CentralizedPHDFilterTaskOptions : public Options {
  public:
   
   DecimalOption* timeBetweenUpdates;
   StringOption*  filterType;
   DecimalOption* survivalProbability;
   DecimalOption* detectionProbability;
   DecimalOption* truncationThreshold;
   IntOption*   maxComponentNumber;
   DecimalOption* mergingThreshold;
   IntOption*   particleNumber;

   DecimalOption* sensorDetectionRay;
   DecimalOption* targetDimension;
   DecimalOption* maxDetectionBearing;

   CentralizedPHDFilterTaskOptions();

   string getObjectName() const {
    return "CentralizedPHDFilterTaskOptions";
   }
 };
 
 
 class CentralizedPHDFilterTask : public Task{
  private:
   
   static const TaskPlate _plate = CPH_TASK;
   
   static const long int  _maxDurSec  = 0,      _maxDurUsec = 100000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 500000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 50000;
   
   PHDFilter* _phdFilter;
   
   CommModule* _commModule;
   
#ifndef MIP_PLATFORM_ARM
   DraWin*      _draWin;   
   unsigned int _drawList; 
   bool         dwOn;      
 #endif

   vector<RangeSens*> _hok;
   ProtrusionsExtr* _protrusionExtractor;
   
   CentralizedPHDFilterTaskOptions _options;
   
   // Define here the local variables of your task.
   int sensorNum;
   vector<PosiFeatures> _posiFeatures;
   vector<Pose> _sensorPose;

   map<int,Pose> _othersPose;
   vector<int> _othersPoseNums;
   map<int,Timer> _timeFromLastMessage;
   Timer _lastStepTime;
   
   // Define here the private methods of your task.
   void _draw();
   
   void communicate();
   
  public:
   CentralizedPHDFilterTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~CentralizedPHDFilterTask();
   
   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();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif