PHDFilterTask.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 __PHD_FILTER_TASK_H_
#define __PHD_FILTER_TASK_H_

#include <armadillo>

#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <GMPHDUnicycle2DIdFilter.h>
#include <GMPHDUnicycle2DIdRCFilter.h>
#include <GMPHDUnicycle2DIdCoopFilter.h>
#include <GMPHDUnicycle2DIdCoopFilter2.h>
#include <GMPHDUnicycle2DIdCoopRCFilter.h>
#include <GMPHDUnicycle2DIdCoop3Filter.h>
#include <GMPHDUnicycle2DIdCoop4Filter.h>
#include <GMPHDUnicycle2DNoIdFilter.h>
#include <GMPHDLinear2DTargetNoIdFilter.h>
#include <GMIDPHDUnicycle2DRCFilter.h>
#include <PHDRansac.h>
#include <PHDIDRansac.h>

#include <SharedMem.h>
#include <CommonOptions.h>
#ifndef MIP_PLATFORM_ARM
 #include <DraWin.h>
#endif
#include <CommModule.h>

#include <EKFilterUnicycle2D.h>
#include <SimTime.h>
#include <OfflineMR.h>

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


namespace MipTasks{
 
 
 enum PHDFilterTaskFunctionalities{
  PHDF_TASK_COMM_MYPOSE,
  PHDF_TASK_COMM_MYCI,
  PHDF_TASK_COMM_MYGM,
  PHDF_TASK_COMM_MYPF,
  PHDF_TASK_COMP_EKF,
  PHDF_TASK_USEOTH_PF,
  PHDF_TASK_SELF_ESTIM,
  PHDF_TASK_ROBOCENTRIC,
  PHDF_TASK_PHD_FILT,
  PHDF_TASK_FUNCTIONALITIES_NUM
 };
 
 static const bool PHDFilterTaskFunctionalitiesTable[PHD_FILTER_TYPE_NUM][PHDF_TASK_FUNCTIONALITIES_NUM]={
//                        COMM_MYPOSE  COMM_MYCI  COMM_MYGM  COMM_MYPF  COMP_EKF  USEOTH_PF  SELF_ESTIM  ROBOCENTRIC  PHD_FILT 
/*LINEAR_NO_ID*/          false,       false,     false,     false,     false,    false,     false,      false,       true,
/*UNICYCLE_2D_ID*/        true,        true,      true,      false,     false,    false,     false,      false,       true,
/*UNICYCLE_2D_ID_RC*/     true,        true,      true,      false,     true,     false,     false,      true,        true,
/*UNICYCLE_2D_ID_COOP*/   true,        false,     false,     true,      false,    true,      true,       false,       true,
/*UNICYCLE_2D_ID_COOP_2*/ true,        false,     false,     true,      false,    true,      true,       false,       true,
/*UNICYCLE_2D_ID_RC_COOP*/true,        false,     false,     true,      false,    true,      false,      true,        true,
/*UNICYCLE_2D_ID_3_COOP*/ true,        false,     false,     true,      false,    true,      true,       false,       true,
/*UNICYCLE_2D_ID_4_COOP*/ true,        false,     false,     true,      false,    true,      true,       false,       true,
/*UNICYCLE_2D_ID_CENTR*/  true,        false,     false,     false,     false,    false,     false,      false,       false,
/*UNICYCLE_2D_NO_ID*/     false,       false,     false,     false,     false,    false,     false,      false,       true,
/*UNICYCLE_2D_ID_RC_CAM*/ true,        false,     false,     false,     false,    false,     false,      true,        true,
/*NO_PHD_EKF_ONLY*/       false,       true,      false,     false,     true,     false,     false,      false,       false,
/*ID_UNICYCLE_2D_RC*/     true,        true,      true,      false,     true,     false,     false,      true,        true
 };
 
 
 
 /* @{ */
 
 class PHDFilterTaskOptions : public Options {
  public:
   
   DecimalOption* timeBetweenMeasUpdates;
   DecimalOption* timeBetweenSysUpdates;
   StringOption*  filterType;
   DecimalOption* survivalProbability;
   DecimalOption* detectionProbability;
   DecimalOption* truncationThreshold;
   IntOption*   maxComponentNumber;
   DecimalOption* mergingThreshold;
   IntOption*   particleNumber;
   
   DecimalOption* sensorDetectionRay;
   DecimalOption* targetDimension;
   DecimalOption* maxDetectionBearing;
   
   StringOption* rootFiles;
   BoolOption* saveGMFiles;
   BoolOption* offlineSimulation;
   
   PHDFilterTaskOptions();
   
   string getObjectName() const {
    return "PHDFilterTaskOptions";
   }
 };
 
 
 class PHDFilterTaskSharedMem : public SharedMem{
  public:
   void initSharedMem();
   
   PosiFeatures getMyProtrusions(MotionModuleTState &state);
   
   void getMyControlInputs(MRControlInputs &ci);
   
   bool doMloc();
   
   void unsetDoMloc();
   
   bool printFormation();
   
   bool printFormationDetails();
   
   void setMyGaussianMixture(string &gm);
   
   void getMyGaussianMixture(string &gm);
 };
 
 
 class PHDFilterTask : public Task{
  private:
   
   static const TaskPlate _plate = PHD_TASK;
   
   static const long int  _maxDurSec  = 0,      _maxDurUsec = 150000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 500000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 200000;
   
   PHDFilter* _phdFilter;
   PHDFilterType _phdFilterType;
   
   MobileRob*  _mobileRob;
   
   CommModule* _commModule;
   
#ifndef MIP_PLATFORM_ARM
   DraWin*      _draWin;   
   unsigned int _drawList; 
   bool         dwOn;      
 #endif
   
   fstream *_saveGMFile;
   
   PHDFilterTaskSharedMem* _sharedMem;
   
   PHDFilterTaskOptions _options;
   
   // Define here the local variables of your task.
   int _myId;
   Pose _myEstimatePose;
   Pose _myPose;
   PosiFeatures _myPosiFeatures;
   
   map<int,PosiFeatures> _othersPosiFeatures;
   vector<int> _othersPosiFeaturesNums;
   
   map<int,GaussianMixtureJointId> _othersGM;
   vector<int> _othersGMNums;
   
   int counter;
   
   MRControlInputs _myCI;
   
   EKFilterUnicycle2D* ekf;
   map<int,EKFilterUnicycle2D*> _ekfs;
   vector<int> _activeEKFs;
   
   PHDRansac* _ransac;
   SimTimer _counterSendGM;
   
   
   map<int, MRControlInput > _othersCI;
   vector<int> _othersVelNums;
   
   
   map<int,Pose> _othersPose;
   vector<int> _othersPoseNums;
   map<int,SimTimer> _timeFromLastMessage;
   SimTimer _lastStepTime;
   
   // Define here the private methods of your task.
   void _draw();
   
   void _communicate();
   
   void _sendMyPose(CommNode &sender, CommNode &recipient);
   
   void _sendMyControlInput(CommNode &sender, CommNode &recipient);
   
   void _sendMyGaussianMixture(CommNode &sender, CommNode &recipient);
   
   void _sendMyPosiFeature(CommNode &sender, CommNode &recipient);
   
   void _receiveMyPose(CommPacket* packet, int senderId);
   
   void _receiveMyControlInput(CommPacket *packet, int senderId);
   
   void _receiveMyGaussianMixture(CommPacket *packet, int senderId);
   
   void _receiveMyPosiFeature(CommPacket *packet, int senderId);
   
  public:
   PHDFilterTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~PHDFilterTask();
   
   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