Tracker.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 __TRACKER_H_
#define __TRACKER_H_

#include <Task.h>
#include <Resource.h>
#include <SharedMem.h>
#include <MobileRob.h>
#include <Time.h>
#include <CommonOptions.h>
#include <TrajControl.h>
#include <Odometry.h>
#include <DraWin.h>
//#include <mobRob2DPIDController.h>

using namespace MipAlgorithms;
using namespace MipResources;


namespace MipTasks{
 
 /* @{ */

 
 enum TrackerState{
  TRACKER_IDLE_STATE,
  TRACKER_FIRST_STATE,
  TRACKER_NORMAL_STATE,
  TRACKER_NUM_STATES
 };
 
 enum TrackerPath{
  TRACKER_ASTEROID_PATH,
  TRACKER_CARDIOID_PATH,
  TRACKER_CLOTHOID_PATH,
  TRACKER_EIGHT_PATH,
  TRACKER_ELLIPSE_PATH,
  TRACKER_SEGMENT_PATH,
  TRACKER_NUM_PATHS
 };
 
 static const char* TrackerPathNames[TRACKER_NUM_PATHS]={
  "Asteroid",
  "Cardioid",
  "Clothoid",
  "Eight",
  "Ellipse",
  "Segment"
 };
 
 enum TrackerTimeLaw{
  TRACKER_STILLNESS_TIMELAW,
  TRACKER_CONST_SPEED_TIMELAW,
  TRACKER_TRIANGULAR_SPEED_TIMELAW,
  TRACKER_TRAPEZOIDAL_SPEED_TIMELAW,
  TRACKER_CUBIC_SPEED_TIMELAW,
  TRACKER_TRAPEZOIDAL_ACC_TIMELAW,
  TRACKER_TRAPEZOIDAL_DEC_TIMELAW,
  TRACKER_NUM_TIMELAWS
 };
 
 static const char* TrackerTimeLawNames[TRACKER_NUM_TIMELAWS]={
  "Stillness",
  "ConstantSpeed",
  "TriangularSpeed",
  "TrapezoidalSpeed",
  "CubicSpeed",
  "TrapezoidalAcc",
  "TrapezoidalDec",
 };
 
 
 class TrackerSharedMem : public SharedMem{
  public:
   
   void initSharedMem(){}
   
   Path* getPath(){
    if(path!=NULL){
    }
    return path;
   }
   
   TimeLaw* getTimeLaw(){
    return timeLaw;
   }
   
   bool getDoTracking(){
    return _doTracking;
   }
   
   void unsetNewPath(){
    _newPath=false;
   }
   
   bool getNewPath(){
    return _newPath;
   }
   
   bool getUseDFL(){
    return _useDFL;
   }
   
   Angle getOrientation(){
    return orientation;
   }
   
 //   SampledTrajectory getSampledTraj(){
 //    return samplTraj;
 //   }
   
 };
 
 
 class TrackerOptions : public Options {
  public:
   StringOption* path;
   StringOption* timeLaw;
   
   IntOption* pathSource;
   IntOption* timeLawSource;
   
   DecimalOption* pathAmplitude;
   DecimalOption* pathHorAxis;
   DecimalOption* pathVertAxis;
   DecimalOption* pathCenterAbs;
   DecimalOption* pathCenterOrd;
   DecimalOption*  pathRotation;
   DecimalOption* pathStartAbs;
   DecimalOption* pathStartOrd;
   DecimalOption* pathEndAbs;
   DecimalOption* pathEndOrd;
   
   DecimalOption* timeLawSpeed;
   DecimalOption* timeLawAcceleration;
   DecimalOption* timeLawJerk;
   IntOption* timeLawCommTime1;
   IntOption* timeLawCommTime2;
   DecimalOption* timeLawStart;
   DecimalOption* timeLawStop;
   
   DecimalOption* trajectoryStart;
   DecimalOption* trajectoryStop;
   
   BoolOption* saveTStates;
   
   TrackerOptions();

   string getObjectName() const {
    return "TrackerOptions";
   }
 };
 
 
 class Tracker : public Task{
  private:
   static const TaskPlate _plate = TKR_TASK;
   static const long int  _maxDurSec  = 0,     _maxDurUsec = 500000;
   static const long int  _maxSamplPeriodSec = 2, _maxSamplPeriodUsec = 0;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 10000;
   
   TrackerOptions options;
   TrackerSharedMem* sharedMem;
   
   vector<Position> _realtrajGL;
   vector<Position> _destrajGL;
   
   Odometry odo;
   
   Path* path;
   TimeLaw* timeLaw;
   
   bool _noSharedMemory;
   
   fstream _saveTState;
   fstream _saveTStateTime;
   
   TrackerState state;
   AnalyticalTrajParam trajParam;
   AnalyticalTrajectory traj;
   TrajControl control;
   //mobRob2DPIDController* pidc;
   
   VelVec command;
   Time passato;
   Timer _timer;
   Timer runTimer;
   TrajectoryState status;
   
   MobileRob* _mobileRob;  
   MotionModuleTState _mrTState;
   
 #ifndef MIP_PLATFORM_ARM
   DraWin*    _draWin;   
   unsigned int _drawList; 
   unsigned int _drawListWake; 
   bool         dwOn;      
 #endif
   
 
   
  public:
   bool first;
   bool last;
   Tracker(ResourcePointers resources,int argc, const char* argv[]);
   
   ~Tracker(){
   }
   
   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 idleRun();
   void normalRun();
   void firstRun();
   
   void switchPath(Path* newPath){
    path=newPath;
   }
   
   void switchTimeLaw(TimeLaw* newTimeLaw){
    timeLaw=newTimeLaw;
   }
   
   void draw();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif