VisualHovering.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 __VISUAL_HOVERING_H_
#define __VISUAL_HOVERING_H_


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

#include <Uav.h>
#include "HBstructs.h"
#include <Time.h>
#include <Camera.h>
#include <VisualTracking.h>

#include <Task.h>
#include <CommonOptions.h>
#ifdef FOUND_VISP_LIBS
    #include <VisTrackVisp.h>
#else
    #include <VisTrackCamshift.h>
#endif

#include <armadillo>

using namespace MipResources;
using namespace MipAlgorithms;

namespace MipTasks{
 
 
 
 
 /* @{ */
 
 enum visualHoveringTypes{
  VISUAL_HOVERING_CAMSHIFT,
  VISUAL_HOVERING_TYPES_NUM
 };
 
 static const char * visualHoveringTypesNames[VISUAL_HOVERING_TYPES_NUM] ={
  "Camshift"
 };

 enum VisualHoveringStates{
  VISUAL_OVERING_FIRST_RUN,
  VISUAL_OVERING_NORMAL_RUN,
  VISUAL_OVERING_LAST_RUN,
  VISUAL_OVERING_STATES_NUM
 };
 
 static const char* VisualHoveringStatesNames[VISUAL_OVERING_STATES_NUM] = {
  "FirstRun",
  "NormalRun",
  "LastRun"
 };

 class VisualHoveringOptions : public Options {
  public:
   // TODO 1: to be removed, actually they are UAV options!
   DecimalOption   *trimRoll;          
   DecimalOption   *trimPitch;         
   DecimalOption   *trimYaw;           
   IntOption       *heightCtrlType;    

   // TODO 2: check do we really need this?
   IntOption       *algoType;          
   
   IntOption       *visDepth;          
   
   // Gains values
   DecimalOption   *inKAngle;          
   DecimalOption   *inKfeatErr;        
   DecimalOption   *inKintegral;       
   BoolOption      *use_omega;     
   
   StringOption    *log_path;          
   StringOption    *log_name;          
   BoolOption      *make_log;          
   BoolOption      *make_video_log;    
   
   VisualHoveringOptions();
   
   string getObjectName() const{
      return "VisualHoveringOptions";
   }
 };


 class VisualHoveringVar{
  public:
   Decimal ControlAngles[2];                   
   short ctrlEnable;
   ofstream    _logFile;
   int         _numrow;
 };
 
 class VisualHovering : public Task {
  private:
   static const TaskPlate _plate = VHO_TASK;  
   
   // Mandatory variables: timing for the scheduler
   static const long int  _maxDurSec      = 0, _maxDurUsec      = 50000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec  = 35000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec  = 25000;
   
   // Reources
   Uav   *_uav;   
   Camera *_cam;   

   // TODO: we should better manage this: make a virtual class to handle different target tracking algorithms
      #ifdef FOUND_VISP_LIBS
          VisTrackVisp* _visTrack;  
      #else
          VisTrackCamshift* _visTrack;  
      #endif
      
  // ########## new Armadillo implementation ##########

      arma::Col<Decimal>::fixed<2> error_arma;
      arma::Col<Decimal>::fixed<2> des_angles_arma;

      arma::Col<Decimal>::fixed<2> feat_arma;
      arma::Col<Decimal>::fixed<2> feat_dot_arma;

      arma::Col<Decimal>::fixed<2> V_est_arma;
      arma::Col<Decimal>::fixed<2> V_des_arma;
      arma::Col<Decimal>::fixed<2> V_des_yaw_arma;

      arma::Col<Decimal>::fixed<3> Omega_arma;

      arma::Mat<Decimal>::fixed<2,2> J_V_arma;
      arma::Mat<Decimal>::fixed<2,3> J_Omega_arma;
      arma::Mat<Decimal>::fixed<2,2> J_V_pinv_arma;
      arma::Mat<Decimal>::fixed<2,2> yaw_rotation_arma;
  // ########## new Armadillo implementation ##########



      CvMat *cv_Omega;

   IplImage*    _frame;   
   ImageFeature  imgFeat;  
   Time t;           
   
   VisualHoveringOptions _options; 
   VisualHoveringVar*  _var;   
   VisualHoveringStates  _state;  
   visualHoveringTypes  _types;  
   
   Decimal Kangle;
   Decimal KfeatureError;
   Decimal KfeatureError_old; // TODO: remove this after complete switch to armadillo
   Decimal Ki;
   Decimal error[2];
   Decimal cRoll, cPitch, cYaw, cPan, cTilt, sRoll, sPitch, sYaw, sPan, sTilt;
   Decimal Jv[4];
   Decimal mass;
   
   Decimal A;
   Decimal B;
   Decimal C;
   Decimal f;
   
   Decimal u;
   Decimal v;
   Decimal uDot;
   Decimal vDot;
   Decimal uInt;
   Decimal vInt;
   
   Decimal thrustControl;
   
   Decimal pan;
   Decimal tilt;
   
   Decimal roll;
   Decimal pitch;
   Decimal yaw;
   
   // TODO: remove this according to TODO1
   Decimal rollTrim;
   Decimal pitchTrim;
   Decimal yawTrim;
   
   // Used in compute control cycle
   Pose3D tmpPose;
   Orientation3D tmpO;
   Position3D    tmpP;
   
   Decimal Omega[3];
   Decimal Jomega[6];
   Decimal pitchDes, rollDes;
   Decimal CommandAngle[2];           // output angles
   
   Decimal Z; 
   
   void _firstRun();
   
   void _normalRun();
   
   void _lastRun();
   
   void ComputeControl();
  public:
   VisualHovering(ResourcePointers resources, int argc, const char* argv[]);
   
   ~VisualHovering();

   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();
 };
}; // end namespace MipTasks






#endif


/* @} */