FormUpdate.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 __FORM_UPDATE_H_
#define __FORM_UPDATE_H_


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

#include <CommonOptions.h>
#include <CommModule.h>

using namespace MipResources;


namespace MipTasks{
 
 /* @{ */
 
 class FormUpdateSharedMem : public SharedMem{
  public:
   void initSharedMem(){
    if (formation == NULL){
     formation = new PoseFeatures;
    }
    if (estimates == NULL){
     estimates = new PoseFeatures;
    }
   }
   
   bool doMloc(){
    return _doMloc;
   }
   
   PoseFeatures getEstimates(){
    return *estimates;
   }
   PoseFeatures getFormation(){
    return *formation;
   }
   void setFormation(PoseFeatures &form){
    *formation = form;
   }
 };
 
 
 class FormUpdate : public Task{
  private:
   static const TaskPlate _plate = FUP_TASK;
   static const long int  _maxDurSec  = 0,     _maxDurUsec = 30000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 200000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 100000;
   
   MobileRob* _mobileRob;  
 //  Position   robPosition; ///TODO
 //  Pose       robPose;     ///TODO
   
   DraWin*      _draWin;   
   unsigned int _drawList; 
   bool         dwOn;      
   
   FormUpdateSharedMem*  _sharedMem;
   
   CommModule* _commModule;
   
   map<int, Pose> othersPose;
 //   map<int, bool> othersPoseActive;
   
   int myId;
   Pose myPose;
   
   PoseFeatures formation;
   
  public:
   FormUpdate(ResourcePointers resources,int argc, const char* argv[]){
    
    // richiedi una risorsa di tipo mobileRob
    _mobileRob = (MobileRob*) ResourceUtilities::macroResFromPlate(resources,MR_INIT,MR_END);
    if(_mobileRob == 0){
     fatalResourceNotPresent(MR_INIT);
    }
    
    // richiedi una risorsa di tipo commModule
    _commModule = (CommModule*) ResourceUtilities::macroResFromPlate(resources,CM_INIT,CM_END);
    if(_commModule == 0){
     fatalResourceNotPresent(CM_INIT);
    }
    // richiedi una box alla risorsa
    _commModule->requestBox(getPlate());
    
    // richiedi una risorsa di tipo SharedMem
    _sharedMem = (FormUpdateSharedMem*) ResourceUtilities::resFromPlate(resources,SHMEM_RES);
    if(_sharedMem == 0){
     fatalResourceNotPresent(SHMEM_RES);
    }
    _sharedMem->initSharedMem();
    
    dwOn = false;
    // controlla se richiesta la stampa su visualizer
    if (commonOptions->embVisOn->getValue() == 1){
     // richiedi la risorsa
     _draWin = (DraWin*) ResourceUtilities::macroResFromPlate(resources,DW_INIT,DW_END);
     // se richiesta ma non presente la risorsa dai un warning
     if(_draWin == 0){
      warningResourceNotPresent(DW_INIT);
     }
     // altrimenti attiva il bool di stampa
     else{
      dwOn = true;
     }
    }
    
    // se stampa attiva
    if (dwOn){
     while(!_draWin->addList(Time(0,1000),_drawList)){
      usleep(1000);
     };
    }
    
    // mi segno il mio Id
    myId = commonOptions->robotId->getValue();
    
    // inizializzazione della fifo
   }
 
   
   ~FormUpdate(){
    othersPose.clear();
 //    othersPoseActive.clear();
   }
 
   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 comunicate();
   
   void computeFormation();
   
   void draw();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif