OfflinerTask.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 __OFFLINER_TASK_H_
#define __OFFLINER_TASK_H_

#include <Task.h>
#include <Resource.h>
#include <CommModule.h>
#include <SharedMem.h>
#include <CommonOptions.h>
#include <GaussianMixture.h>
#include <OfflineMR.h>

#include <SpacesArma.h>

// Inclusion of offline UAV
#include <OfflineUav.h>

#ifndef MIP_PLATFORM_ARM
 #include <DraWin.h>
#endif
#include <SimTime.h>

// #include <libxml/tree.h>
// #include <libxml/parser.h>

using namespace MipResources;

namespace MipTasks{
 
 /* @{ */
 
 
 enum OfflineJobs{
  OJ_PARTICLE_MUT_LOC, 
  OJ_FAST_SLAM_PMLNMR, 
  OJ_PHDFILTER_STAGE1, 
  OJ_PHDFILTER_STAGE2, 
  OJ_PHDFILTER_QUAD, 
  OFFLINE_JOBS_TYPES_NUM
 };
 
 static const char* OfflineJobsNames[OFFLINE_JOBS_TYPES_NUM] = {
  "ParticleMutLoc",
  "FastSLAMMutLoc",
  "PHDFilter_Stage1",
  "PHDFilter_Stage2",
  "PHDFilter_Quad"
 };

 
 enum OfflinerFunctionalities{
  OFF_SHME_MYPF, 
  OFF_SHME_MYCO, 
  OFF_SHME_MYCI, 
  OFF_SHME_MYGM, 
  OFF_COMM_OTPOSE, 
  OFF_COMM_OTPF, 
  OFF_COMM_OTCO, 
  OFF_COMM_OTCI, 
  OFF_COMM_OTGM, 
  OFF_EMU_UAV,  
  OFFLINER_FUNCTIONALITIES_NUM
 };
 
 static const bool OfflinerFunctionalitiesTable[OFFLINE_JOBS_TYPES_NUM][OFFLINER_FUNCTIONALITIES_NUM]={
//                       SHME_MYPF SHME_MYCO SHME_MYCI SHME_MYGM COMM_OTPOSE COMM_OTPF COMM_OTCO COMM_OTCI COMM_OTGM EMU_HOV
/*OJ_PARTICLE_MUT_LOC*/  true,     false,    false,    false,    true,       true,     false,    false,    false,    false,   
/*OJ_FAST_SLAM_PMLNMR*/  true,     false,    false,    false,    true,       true,     false,    false,    false,    false,
/*OJ_PHDFILTER_STAGE1*/  true,     false,    false,    false,    true,       false,    false,    false,    false,    false,
/*OJ_PHDFILTER_STAGE2*/  false,    false,    true,     true,     false,      false,    false,    true,     true,     false,
/*OJ_PHDFILTER_QUAD*/    false,    false,    false,    false,    true,       false,    false,    false,    false,    true
 };
 
 
 enum OfflinerTaskStates{
  OFFLINER_TASK_FIRST_RUN,
  OFFLINER_TASK_NORMAL_RUN,
  OFFLINER_TASK_STATES_NUM
 };
 
 static const char* OfflinerTaskStatesNames[OFFLINER_TASK_STATES_NUM] = {
  "FirstRun",
  "NormalRun"
 };
 
 
 class OfflinerTaskOptions : public Options {
  public:
   StringOption*       rootFiles;
   StringOption*       rootFileIds;
   StringOption*       rootFileFeatSet;
   StringOption*       rootFileTState;
   StringOption*       rootFileGaussianMixture;
   StringOption*       rootFileControlInput;
   StringOption*       rootFileTruth;
   StringOption*       rootFileTruthTSt;
   StringOption*       rootImPlaneMeasFile;
   DecimalOption*       startTime;
   DecimalOption*       endTime;
   DecimalOption*       timeStep;
   BoolOption*        stepByStep;
   StringOption*       jobType;
   Pose3DOption*       startPoseGroundTruthQuad;
   Pose3DOption*       relGroundTruthQuadMobRob;
   DecimalArrayOption*    rotoAxisFromGroundToQuad;
   BoolOption*        useStaticQuad;
   BoolOption*        saveGTFile;
   BoolOption*        saveImPlaneMeas;
   IntOption*         GTTypeData;
   
   // Uav files
   StringOption*       UavId;
   StringOption*       UavSonarAttFile;
   StringOption*       UavVelFile;
   StringOption*       UavObsFile;
   StringOption*       UavTimeFile;
   StringOption*       UavGTFile;
   StringOption*       UavGTTimeFile;
   
   Pose3DOption*       UavMRGTRelPose;
   
   DecimalOption* widthCam;
   
   DecimalOption* heightCam;
   
   DecimalOption* focaLengthCam;
   
   Pose3DOption* relQuadCam;
   
   PositionOption* principalPoint;
   
   StringOption*  rootSaveFolder;
   
   BoolOption*        setAbsoluteStartTime;
   
   OfflinerTaskOptions();

   string getObjectName() const {
    return "OfflinerTaskOptions";
   }
 };
 
 
 class OfflinerTaskSharedMem : public SharedMem{
  public:
   
   
   void setTime(Time &t);
   
   void setMyProtrusions(PosiFeatures &protrusions, MotionModuleTState &protrusionsTState);
   
   void setMyCamObs(vector<Angle> &camObs, MotionModuleTState &camObsTState);
   
   
   void setGroundTruthMobRobQuad(const Decimal &roll, const Decimal &pitch, const PosiFeatures3DTimeArma& pos, const Roto3DArma *rotGT, vector<int> &ids);
   
   void setGroundTruthMobRobQuad(const map<int,Pose> &gtPose);
   
   void initSharedMem();
   
   void doMloc(){
    _doMloc = true;
   }
   
   void setMyGaussianMixture(string &gm);
   
   void setMyControlInput(MRControlInputs &ci);
   
   // From now on, the methods are for UAV
   
   void setMyAttitude(const Decimal &ro, const Decimal &pit);
   
   void setMyAttitude(const Angle &ro, const Angle &pit);
   
   void setMyYaw(const Decimal &y);
   
   void setMyYaw(const Angle &y);
   
   void setMyVelocity(const Decimal& lvx, const Decimal& lvy, const Decimal& lvz,const Decimal &avx,const Decimal &avy,const Decimal &avz, const Time &time);
   
   void setMyVelocity(const Decimal& lvx, const Decimal& lvy, const Decimal& lvz,const Decimal &avx,const Decimal &avy,const Decimal &avz);
   
   void setMyVelocity(const Velocity3DArma& lv, const Velocity3DArma& av);
   
   void setMyVelocity(const Velocity3DArma& lv, const Velocity3DArma& av, const Time &time);
   
   
   void setMy3DObs(const vector<Position2DArma> &obs);
   
   void setMy3DObs(PosiFeatures3DTimeArma &camObs, const Roto3DArma &aMat, const Position3DArma &bVec, const Decimal &width, const Decimal &height);
   
   void setMy3DObs(PosiFeatures3DTimeArma &camObs);
    
   void setMyHeight(const Decimal &hei);
 };
 
 
 class OfflinerTask : public Task{
  private:
   fstream fileStream;
   
   OfflineJobs _job;
   
   
   static const TaskPlate _plate = OFF_TASK;
   
//    static const long int  _maxDurSec  = 0,      _maxDurUsec = 100000; 
//    static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 500000;
//    static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 400000;
   
   
   static const long int  _maxDurSec  = 0,      _maxDurUsec = 150000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 500000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 200000;
   
   
   CommModule* _commModule;
//    MobileRob*  _mobileRob;
   
 #ifndef MIP_PLATFORM_ARM
   DraWin*      _draWin;   
   unsigned int _drawList; 
   bool         dwOn;      
 #endif
   
   OfflinerTaskSharedMem* _sharedMem;
   
   OfflinerTaskOptions _options;
   
   // Define here the local variables of your task.
   OfflinerTaskStates    _state;
   Time           _ctime;
   SimTime         _stime;
   SimTime         _prevStime;
   
   vector<int>       _IdsF;
   int           _robotId;
   int           _myIdPos;
   int           _myId;
   MotionModuleTState   _myTState;
   
   map<int, OfflineMR*>  _robots;
/*   
   map<int, OfflineMR> _robotsGT;*/
   
   OfflineMR        _my3DObs;
   
   OfflineUav*       _myUav;
   
   Roto3DArma*       _rotoAxisGroundQuad;
   Roto3DArma*       _calibMatrix;
   Roto3DArma*       _rotoQuadCam;
   Position3DArma*     _posApp;
   Roto3DArma*       _matApp;
   vector<Roto3DArma>   rotoActFromGroundTruthQuadToQuad;
   vector<Position3DArma> traslActFromGroundTruthQuadToQuad;
   
   // Attitude and sonar info
   Decimal         _roll;
   Decimal         _pitch;
   Decimal         _yaw;
   Decimal         _height;
   Velocity3DArma     _linVelUav;
   Velocity3DArma     _angVelUav;
   Roto3DArma*       _rotAppGTOri;
   vector<Position2DArma> _obs;
   
   Roto3DArma*       _rotUavMRGT;
   Position3DArma*     _posUavMRGT;
   
   void _firstRun();
   
   map<int,fstream*>    _gtStreams;
   
   fstream*        _imPlaneMeas;
   
   map<int,Pose>      _gtMobRobQuad;
   
   vector<Position2DArma> _myObs;
   
   TaskOutputs _normalRun();
   
  public:
   OfflinerTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~OfflinerTask();
   
   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();
   
  private:
   
   void _readIdsFile();
   
   void _sendMessages(int hisId);
   
   void _writeSharedMem();
   
   void _writeSharedMemUav();
   
   void _syncCamObs();
   
   void _draw();
   
   map<int,Pose> _computeGT(const PosiFeatures3DTimeArma& pos, int index);
   
   void _computeGTDyn();
   
   void _saveGT();
   
   void _saveImPlaneMeas();
 };
 
 /* @} */
 
};// end namespace MipTasks



#endif