Localization3D.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 __LOCALIZATION_3D_H_
#define __LOCALIZATION_3D_H_


#include <Task.h>
#include <Resource.h>
#include <MobileRob3D.h>
#include <DraWin.h>
#include <SharedMem.h>
#include <Time.h>

#include <CommonOptions.h>
#include <MultiReg.h>
#include <DAEKF.h>
#include <PaFil.h>
#include <CommModule.h>
#include <Kalman.h>
#include <KalmanImu.h>

using namespace MipAlgorithms;
using namespace MipResources;

namespace MipTasks{
 
 /* @{ */
 
 class Localization3DOptions : public Options 
 {
  public:
  
   //IntOption* particleNumber;
   StringOption* nameSurfaceFile;
  
   Localization3DOptions()
   {

    nameSurfaceFile = createStringOption("localization3DNameSurfaceFile","Name of the surface file; default = null ", "");
    
    updateValues();

   }
  
   OptionGroupsType getGroup();
   string getObjectName() const {
    return "Localization3DOptions";
   }
 };
 
 
 class Localization3DSharedMem : public SharedMem
 {
  public:
   void initSharedMem();
 };
 
 
class Localization3D : public Task
{
 private:
   static const TaskPlate   _plate = L3D_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;
   
   MobileRob3D* _mobileRob3D;
   
 #ifndef MIP_PLATFORM_ARM
   DraWin*      _draWin;
   unsigned int _drawList;
   bool         _dwOn;
 #endif
   
   Localization3DSharedMem* _sharedMem;
   Localization3DOptions   _options;
   //KalmanFilter*   _kalmanFilter;
   KalmanFilterImu*  _kalmanFilterImu;
   
   int _myId;
   Pose _myPose;
   
   void draw();
   bool IMU_MeasAvail;//1 = IMU measure available; 0 = IMU measures not available;
   
  public:
   Localization3D(ResourcePointers resources,int argc, const char* argv[]);
   
   ~Localization3D();
   
   TaskPlate getPlate() const
   {
    return _plate;
   }
   
   Time getMaxDuration()
   {
    return Time(_maxDurSec,_maxDurUsec);
   }
   
   Time getMaxSamplPeriod()
   {
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod()
   {
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   
   void set_IMU_MeasAvail()
   {
           IMU_MeasAvail = true;
    return;
   }

   void unset_IMU_MeasAvail()
   {
           IMU_MeasAvail = false;
    return;
   }

   bool get_IMU_MeasStatus()
   {
    return IMU_MeasAvail;
   }

   TaskOutputs run();
 };
 /* @} */
 
};// end namespace MipTasks

#endif