ParticleMutLocTaskNoMR.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 __PARTICLE_MUT_LOC_TASK_NOMR_H_
#define __PARTICLE_MUT_LOC_TASK_NOMR_H_


#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <DraWin.h>
#include <SharedMem.h>
#include <SimTime.h>

#include <CommonOptions.h>
#include <PaFilBFLRelRelNoMR.h>
#include <MutLocFASTSlam.h>
#include <CommModule.h>
#include <Gaussian.h>


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


namespace MipTasks{
 /* @{ */
 
 
 enum FrameTypesPMLNMR{
  FIXED_FRAME_PMLNMR,
  ATTACHED_FRAME_PMLNMR,
  FRAME_TYPES_NUM_PMLNMR
 };
 
 static const char* FrameTypesNamesPMLNMR[FRAME_TYPES_NUM_PMLNMR] = {
  "fixed",
  "attached"
 };
 
 enum MutLocNoMRTypes{
  MAX_LIK_PMLNMR,
  FAST_SLAM_PMLNMR,
  MUTLOC_NOMR_TYPES_NUM
 };
 
 static const char* MutLocNoMRNames[MUTLOC_NOMR_TYPES_NUM] = {
  "MaximumLikelyhood",
  "FastSlam"
 };
 
 
 
 enum PMLNOMRTaskFunctionalities{
  PMLNOMR_TASK_COMM_MYPOSE,
  PMLNOMR_TASK_COMM_MYPF,
  PMLNOMR_TASK_SEP_FILTERS,
  PMLNOMR_TASK_SING_FILT,
  PMLNOMR_TASK_FUNCTIONALITIES_NUM
 };
 
 static const bool PMLNOMRTaskFunctionalitiesTable[MUTLOC_NOMR_TYPES_NUM][PMLNOMR_TASK_FUNCTIONALITIES_NUM]={
//                        COMM_MYPOSE  COMM_MYPF  SEP_FILTERS  SING_FILT  
/*MAX_LIK_PMLNMR*/        true,        false,     true,        false,     
/*FAST_SLAM_PMLNMR*/      true,        true,     false,       true      
 };

 
 
 class ParticleMutLocNoMROptions : public Options {
  public:
   
   IntOption* minObservationSize;
   
   IntOption* findTarget;
   IntOption* targetId;
   
   StringOption* myFrameType;
   StringOption* hisFrameType;
   
   StringOption* algoType;
   
   BoolOption* saveEstimates;
   StringOption* saveFilesRoot;
   BoolOption* offlineSimulation;
   
   ParticleMutLocNoMROptions();
   
   OptionGroupsType getGroup();
   
   string getObjectName() const {
    return "ParticleMutLocNoMROptions";
   }
 };
 
 
 class ParticleMutLocNoMRSharedMem : public SharedMem{
  public:
   void initSharedMem();
   
   PosiFeatures getMyProtrusions(MotionModuleTState &state);
   
   Time getTime();
   
   void setFormation(PoseFeatures &form);
   
   void setEstimates(PoseFeatures &est);
   
   Pose getTarget();
   
   void setTarget(Pose &t);
   
   bool doMloc();
   
   bool printFormation();
   
   bool printFormationDetails();
   
   vector<int> getAvoidParticleMutLocIds();
 };
 
 
 class ParticleMutLocTaskNoMR : public Task{
  private:
   static const TaskPlate _plate = PDE_TASK;
   static const long int  _maxDurSec  = 0,     _maxDurUsec = 500000;
   static const long int  _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 0;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 400000;
   
   MutLocNoMRTypes _algoType;
   
   MutLocFASTSlam* _mlFastSlam;
   
   // resources
   MobileRob*        _mobileRob;  
   DraWin*          _draWin;   
   unsigned int       _drawList; 
   bool           _dwOn;      
   ParticleMutLocNoMRSharedMem* _sharedMem;
   CommModule*        _commModule;
   
   FrameTypesPMLNMR _myFrameType;
   FrameTypesPMLNMR _hisFrameType;
   
   // options
   ParticleMutLocNoMROptions _options;
   
   // algorithms
   map<int,MutLocFilter*> _filter;
   MutLocFilter* _staticFilter;
   map<int,SimTimer> _timeFromLastMeasureUpdate;
   vector<int>       _activatedFilters;
   
   // funzionalities
   SimTime    _actualTime;
   bool    _dofilter;
   vector<int> ignoreIds;
   bool    _printFormation;
   bool    _printFormationDetails;
   
   // self information
   int      _myId;
   Pose     _myPose;
   PosiFeatures _myPosiFeatures;
   Observation  _myObservation;
   
   // others information
   map<int,PosiFeatures> _othersPosiFeatures;
   vector<int>      _othersPosiFeaturesNums;
   map<int,Pose>     _othersPose;
   vector<int>      _othersPoseNums;
   
   // results
   map<int,Pose>  _estimates;
   map<int,Pose>  _estimatesBest;
   vector<int>   _activatedEstimates;
   map<int,SimTimer> _timeFromLastMessage;
   PoseFeatures  _formation;
   PoseFeatures  _formationBest;
   
   fstream _savefile;
   fstream _savefileBest;
   map<int, fstream*> _saveFileAll;
   fstream _savefileAllSt;
   
  public:
   ParticleMutLocTaskNoMR(ResourcePointers resources,int argc, const char* argv[]);
   
   ~ParticleMutLocTaskNoMR();
 
   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 _readSharedMemory();
   
   void _communicate();
   
   
   void _sendMyPose(CommNode &sender, CommNode &recipient);
   
   void _sendMyPosiFeature(CommNode &sender, CommNode &recipient);
   
   void _receiveMyPose(CommPacket* packet, int senderId);
   
   void _receiveMyPosiFeature(CommPacket *packet, int senderId);

   
   
   
   
   
   void _draw();
   
   void _writeSharedMem();
   
   void _computeFormation();
   
   void _insertValidObservations(vector<Observation> &allObservations);
   
   void _completeMutLocFilInput(MutLocFilInput &fStepIn, int hisId);
   
   bool _activeFilter(int hisId);
   
   bool _activeEstimate(int hisId);
   
   void _activateFilter(int hisId);
   
   void _activateStaticObstacleFilter();
   
//    vector <bool> _associateMeasures(int filter);
   
   map <int, vector<Decimal> > _associateMeasures();
   
   map <int, vector<Decimal> > _associateMeasures2();
 };
 
 /* @} */
 
};


#endif