PHDCamFilterTask.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_CAM_FILTER_TASK_H_
#define __PHD_CAM_FILTER_TASK_H_

#include <SpacesArma.h>

#include <Task.h>
#include <Resource.h>
#include <Uav.h>
#include <GMPHDUnicycle2DCamIdRCFilter.h>
#include <PHDRansac.h>

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


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


namespace MipTasks{
 
 /* @{ */
 
 class PHDCamFilterTaskOptions : 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;
   
   DecimalOption*    widthCam;
   
   DecimalOption*    heightCam;
   
   DecimalOption*    focaLengthCam;
   
   Pose3DOption*     relPoseCamQuad;
   
   Pose3DOption*     relPoseSonarQuad;
   
   PositionOption*    principalPoint;
   
   DecimalArrayOption*  rotoQuadMobRob;
   
   IntOption*      saveResults;
   
   DecimalOption*    sigmaNoiseDist;
   
   DecimalOption*    noiseRangeDist;
   
   StringOption*     rootFolder;
   
   StringOption*     rootEstimates;
   
   StringOption*     rootBestEstimate;
   
   StringOption*     rootMeasImPlane;
   
   BoolOption*      addNoiseOnMeas;
   
   BoolOption*      vertZSonarAlgo;
   
   PHDCamFilterTaskOptions();
   
   string getObjectName() const {
    return "PHDCamFilterTaskOptions";
   }
 };
 
 
 class PHDCamFilterTaskSharedMem : public SharedMem{
  private:
   Odometry2DTimeArma odo;
  public:
   void initSharedMem();
   
   PosiFeatures getMyProtrusions(MotionModuleTState &state);
   
   void getMyProtrusionsCam(vector<Position2DArma> &pos);
   
   void getMyProtrusionsCamWithNoise(vector<Position2DArma> &pos, const Decimal &sigmaDist, const Decimal &noiseDist, const Decimal &w, const Decimal h);
   
   void getMyControlInput(Decimal &lvx, Decimal &lvy, Decimal &lvz, Decimal &avx, Decimal &avy, Decimal &avz, Time &ct);
   
   void getMyControlInput(Velocity3DArma &lv, Velocity3DArma &av, Time &ct);
   
   void getMyControlInput(Odometry3DTimeArma &odoTime);
   
   void getMyAttitude(Angle &roll, Angle &pitch);
   
   void getMyYaw(Angle &y);
   
   void getMyHeight(Decimal &height);
   
   void getGT(map<int,Pose> &gtPos);
   
   bool doMloc();
   
   void unsetDoMloc();
   
   bool printFormation();
   
   bool printFormationDetails();
 };
 
 
 class PHDCamFilterTask : public Task{
  private:
   
   // Reference to file to save data
   fstream filozzo2;
   
   // Variable to not call every time the option
   Decimal noiseDist;
   Decimal sigmaDist;
   
   // Stream to save data
   fstream* _savEstimates;
   fstream* _saveBestGuess;
   fstream* _saveWeightedSumImPlane;
   fstream* _saveImPlaneMeas;
   fstream* _saveImPlaneAllMeas;
   fstream* _saveUAVData;
   
   static const TaskPlate _plate = PHC_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;
   
//    static const long int  _maxDurSec  = 0,      _maxDurUsec = 150000;
//    static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 500000;
//    static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 50000;
   
   PHDFilter*            _phdFilter;
   PHDFilterType           _phdFilterType;
   
   Uav*               _uav;
   
   CommModule*            _commModule;
   
#ifndef MIP_PLATFORM_ARM
   DraWin*                 _draWin;   
   unsigned int            _drawList; 
   bool                    dwOn;      
 #endif
   
   PHDCamFilterTaskSharedMem*     _sharedMem;
   
   PHDCamFilterTaskOptions      _options;
    
   // Define here the local variables of your task.
   int                _myId;
   Pose                _myPose;
   PosiFeatures            _myPosiFeatures;
   vector<Position2DArma>       _myCamObs;
   Odometry3DTimeArma         _myOdo;
   Angle               _roll, _pitch, _yaw;
   Decimal              _height;
   
   map<int,PosiFeatures>       _othersPosiFeatures;
   vector<int>            _othersPosiFeaturesNums;
   
   map<int,GaussianMixtureJointId>  _othersGM;
   vector<int>            _othersGMNums;
   
   int                counter;
   
   PHDRansac             _ransac;
   
   map<int, Decimal >         _othersLVel;
   map<int, Decimal >         _othersAVel;
   map<int, SimTime >         _othersTimeCIn;
   vector<int>            _othersVelNums;
   
   map<int,Pose>           _othersPose;
   vector<int>            _othersPoseNums;
   map<int,SimTimer>         _timeFromLastMessage;
   SimTimer              _lastStepTime;
   
   // If one wants to load ground truth data from shared memory
//    map<int,Pose>           _gtMobRobQuad;
   
   GaussianMixtureJointId      _gmStepResult;
   
   // Define here the private methods of your task.
   void _draw();
   
   void _communicate();
   
   void _receiveMyPose(CommPacket* packet, int senderId);
   
   void _receiveMyControlInput(CommPacket *packet, int senderId);
   
   void _receiveMyGaussianMixture(CommPacket *packet, int senderId);
   
   void _receiveMyPosiFeature(CommPacket *packet, int senderId);
   
   void _readSharedMem();
   
   void _saveOnFile();
   
  public:
   PHDCamFilterTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~PHDCamFilterTask();
   
   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