Pursuing.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 __PURSUING_H_
#define __PURSUING_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <fstream>

#include <R2.h>
#include <Grid.h>
#include <Scan.h>

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

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

using namespace MipResources;

namespace MipTasks{
 
 /* @{ */

 
 class PursuingOptions : public Options {
  public:
 //    DecimalOption* minSamplTime;
   IntOption* customDebugLevel;
 
 
   PursuingOptions(){
 //    minSamplTime = new DecimalOption("scanMatchingSampleTime", "Suggested time between two scans to match, in seconds, <= 0.0 means never checked.", 2.0);
     customDebugLevel = new IntOption("PursuingDebugLevel", "Custom debug level of the Pursuing task.", 3);
 
 //    
 //    list.push_back(minSamplTime);
   }
 };

 // /// \class PursuingSharedMem
// /// \brief Shared memory for the scan matching.
// /// \author Antonio Franchi, Joey Durham and Arnaud Dufranne
// class PursuingSharedMem : public SharedMem{
//  public:
//   /// init function for the PursuingSharedMem
//   void initSharedMem(){
//   }
// };

 typedef std::vector<Position > PositionPath;
 typedef std::vector<Position > ViewPoints;
 
 int gMinArcSize = 2;
 int gMinMergeArcSize = 1;
 int gMinRadialArcSize = 3;
 
 
 class Pursuing : public Task{
  private:
   static const TaskPlate _plate = PUR_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;
   CommModule* _commModule;
   PursuingComm *_pursuingComm;
   
   uint _robotId;
 //   PursuingSharedMem* _sharedMem;
   
   PursuingOptions _options;
   
   PursuingStates _state;
   PursuingGuardStates _frontGuardSubState; //substate for the frontierGuard
   PursuingGuardUpdatingStates _frontGuardUpdatingSubState; //substate for the Updating Frontier Guard
   PursuingWandererStates _wandererSubState; //substate for the wanderer
   
   Frontier _frontier;
   Frontier _freeArcs;
   Frontier _prevFrontier;
   Grid _LSR;
   Grid _prevLSR;
   bool _bFrontierInitialized;
   
   //Pose _goalPose;
   int _iLeader;
   //int _iWaitForAgent
   
   // If an int so we can set it to -1 when not waiting, but have to check when using it that it's > 0
   int _iWaitForAgent;// i wait for an agent
 //   uint _waitedForAgent;//id of the agent we wait for
   
   map<uint,PursuingMessageTypes > _asynRequests;//list of asyncrequest, documentation in _updateGuardList
   vector<uint> _followers;
   map<uint,GuardUpdatingState> _localGuards; //the table containing the local guards and their state in the protocol
   map<uint,PursuingStates> _neighborStates; //the last received States of the neighbours
   map<uint,TimeStampedPose> _neighborPoses; // the last received Poses of the neighbours
 
   void _expand( );
   
   void _guardUpdating( ); //subfunction of _guard()
   void _guardPlanning(); //subfunction of _guard()
   void _guardIdleSendingFollower( ); //subfunction of _guard()
   void _guardIdle( ); //subfunction of _guard()
   
   void _follow( );
   void _wander( );
   void _wanderWaiting();
   
   void _recordLSR();
   Position _pickBestViewpoint( uint *iNumViewPoints );
   bool _isCellCovered( DubInt iViewPoint, DubInt iCell );
   ExplorationParams _getExplParams();
   
   
 
   void _setToExpand( Pose goalPose );
   
   string getObjectName() const {
    stringstream ss;
    assert(getPlate()<TASK_NUM);
    
    ss << TaskNames[getPlate()] << ", Agent : "<< getId();
    return ss.str();
   }
   
   void _setToGuard( );
   
   void _setToFollow( uint leaderId );
   
   void _setToWander( );
   void _setToWanderWaitingForLeader( uint leaderId );
   
   void _printNeighbouringGuards();
   
   void _printAsyncRequests();
   
   void _updateAsynRequests();
   
   void _updateGuardList();
   
   bool _guardUpdatingDone();
   
   void _receiveGimmeYourFrontier(int &robIdTosendTo,bool &hasToSendFrontier);
   
   void _checkLocalGuardsArray(int &robIdToSendTo,bool &hasToSendFrontier );
   
   void _receiveFrontiers();
   
   void _receiveUpdatedFrontier();
 
   void _commonActions();
   
   bool _isWaitingForAnyAgent();
   
   void _updateFrontier();
   
   void _processFrontier();
  
  public:
   Pursuing(ResourcePointers resources,int argc, const char* argv[]);
   
   ~Pursuing();
   
   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();
   
   uint getId();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif