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 <Message.h>

#include <CommonOptions.h>

#include <ObsAvoiAlgoSND.h>

using namespace MipResources;
using namespace MipBaselib;
using namespace MipAlgorithms;

namespace MipTasks{
    
/* @{ */


class PursuingOptions : public Options {

 public:
  IntOption*  customDebugLevel;
  DecimalOption* maxSpeed;
  DecimalOption* maxRotSpeed;
  DecimalOption* maxRange;
  BoolOption* forceHokuyoPar;
   
  PursuingOptions();

  string getObjectName() const {
   return "PursuingOptions";
  }
};

class PursuingSharedMem : public SharedMem{
 public:
  void initSharedMem();
  
  PoseFeatures getFormation();
  
  PoseFeatures getEstimates();
  
  bool doAutoNavigation();
  
  void setAvoidMutLocIds(vector<int> &newIds);
  
  vector<int> getAvoidMutLocIds();
  
  void clearAvoidMutLocIds();
};
  

enum PursuingState
{
  WAITING,
  INITIALIZE,
  RUN
};

class Pursuing : public Task, public MessageHandler{
 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;
  ObsAvoiAlgoSND* _pObsAvoiAlgo;
  
  PursuingState _state;
  uint _robotID;
  PursuingSharedMem* _sharedMem;
  
  PoseFeatures _otherRobotPoses;        

  Pose _goal;
  pthread_mutex_t _goalMutex;
  bool _bSendScanFromGoal;
  string _centralIP;
  
  int _count;

  RangeSensPar* _sensorParams;
  Decimal _rangeMax; //TODO remove it
  Decimal _robotRadius;
  PursuingOptions _options;
  
  bool _getScan(Scan &scan, Time waitTime = Time(0,0));
  Pose _getPose();
  Pose _getGoal();
  void _setGoal( Pose newGoal );
  bool _atGoal();
  
  void _initialize();
  void _recordOtherRobotPoses();
  void _navigate();
  void _communicate();
  
  void _sendPoseAndScan(string ip);
  void _sendPose(string ip);
  void _sendPoseArray(string ip);
  void _sendParams(string ip);
  
  string getObjectName() const {
    stringstream ss;
    assert(getPlate()<TASK_NUM);
    
    ss << TaskNames[getPlate()] << ", Agent : "<< getID();
    return ss.str();
  }
  
 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() const;
  
  Decimal getSensorLinRangeMax();
  Angle getSensorFieldOfView();
  int getSensorNumRays();
  Angle getSensorAngRes();
  Decimal getRobotRadius();
  
  void handleMessage(string inMessage,string ip);
};

/* @} */
    
};// end namespace MipTasks

#endif