UAVPositioningSystem.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011  Antonio Franchi and Luca Ricci
//
// 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 __UAV_POSITIONING_SYSTEM_TASK_H_
#define __UAV_POSITIONING_SYSTEM_TASK_H_

#include <Task.h>
#include <Resource.h>
#include <VisualOdometryPTAMM.h>
#include <CommonOptions.h>
#include <Time.h>
#include <Camera.h>
#include <Uav.h>
#include "HBstructs.h"
#include <Keyboard.h>
#include <TooN/se3.h>
#include <TooN/TooN.h>
#include <math.h>
#include <Imu_sensor/Imu_sensor.h>
#include <LocUavFilter.h>

#ifndef MIP_PLATFORM_ARM
#include <DraWin.h>
#endif

using namespace MipResources;
using namespace MipAlgorithms;
using namespace TooN;

namespace MipTasks{

 /* @{ */

        enum UAVPositioningSystemTypes{
  UAV_POSITIONING_SYSTEM_TYPE,
                UAV_POSITIONING_SYSTEM_TYPE_NUM
 };

        static const char * UAVPositioningSystemTypesNames[UAV_POSITIONING_SYSTEM_TYPE_NUM] ={
  "type"
 };

 enum UAVPositioningSystemStates{
  UAV_POSITIONING_SYSTEM_FIRST_RUN,                
  UAV_POSITIONING_SYSTEM_NORMAL_RUN,
                UAV_POSITIONING_SYSTEM_PTAMM_FIRST_STATE,
                UAV_POSITIONING_SYSTEM_PTAMM_SECOND_STATE,
                UAV_POSITIONING_SYSTEM_PTAMM_THIRD_STATE,
                UAV_POSITIONING_SYSTEM_PTAMM_FOURTH_STATE,
                UAV_POSITIONING_SYSTEM_PTAMM_FIFTH_STATE,
                UAV_POSITIONING_SYSTEM_PTAMM_NAVIGATION,
                UAV_POSITIONING_SYSTEM_LAST_RUN,
  UAV_POSITIONING_SYSTEM_STATES_NUM
 };

 static const char* UAVPositioningSystemStatesNames[UAV_POSITIONING_SYSTEM_STATES_NUM] = {
  "FirstRun",
  "NormalRun",
                "LastRun"
 };


 class UAVPositioningSystemOptions : public Options {
  public:
   DecimalOption* decimalOption;
   IntOption*   intOption;
   StringOption* LogFilePathOption;
                        //ESEMPIO DA VISUAL HOVERING
                        //StringOption    *log_path;          ///< Path top save log file.
                        //StringOption    *log_name;          ///< Name of log file.
   UAVPositioningSystemOptions();

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

 class UAVPositioningSystemVar{
  public:
 };

 class UAVPositioningSystem : public Task{
  private:

   /* MANDATORY VARIABLES OF MIP TASKS */
   static const TaskPlate _plate = UPS_TASK;

   static const long int  _maxDurSec  = 0,      _maxDurUsec = 800000;
   static const long int  _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 800000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 500;
   /* ------------------------------- */

                        /* PRIVATE VARIABLES OF THE TASK */
                        UAVPositioningSystemOptions _options;
                        UAVPositioningSystemVar*    _var;
                        UAVPositioningSystemStates  _state;
                        bool _stateTransition;

                        // RESOURCES
   Uav  *_uav;          
   Camera *_cam;   
                        Keyboard* _keyboard;        
                        Imu_sensor* _imu;

                        // ALGORITHMS
   // Visual Odometry communication module
                        VisualOdometryPTAMM* _vOdo;
   // Ekf implementation
   LocUavFilterVar* filterVars;
   LocUavFilterParam* filterParams;
   RootEKF*  filter;

                        // MEASUREMENTS
                        // Variables for PTAMM's measurements
                        Time t;      
                        IplImage*_frame;                                
                        Pose3DandTime ptammRecData;                     
                        Pose3DandTime ptammSndData;                     
                        Time ptammMeasTime;
                        Decimal ptammDelay;

                        // Variables for Gazebo's measurements
                        Decimal uav_acc_x, uav_acc_y, uav_acc_z;   
                        Decimal uav_vel_x, uav_vel_y, uav_vel_z;   
                        Decimal uav_pos_x, uav_pos_y, uav_pos_z;   
                        Decimal uav_roll, uav_pitch, uav_yaw;      
                        Decimal uav_omegaX, uav_omegaY, uav_omegaZ;
                        // Some variables to compute acceleration from Gazebo
                        // previous velocity, previous time, time delta
                        Decimal uav_prevVel_x, uav_prevVel_y, uav_prevVel_z;
                        Time uav_time;
                        Time uav_prevTime;
                        Decimal uav_simDeltaTime;
                        // Trajectory assigned in GAZEBO
                        Decimal traj[3];
                        // PTAMM INITIALIZATION
                        // Variables to compute PTAMM scale (UAV start position)
                        // Variables for realigning PTAMM's frame to Gazebo's frame
                        Pose3DandTime uavStart, uavEnd;
                        Decimal ptammScale; 
                        Decimal diff_x, diff_y, diff_z, diff_roll, diff_pitch, diff_yaw;
                        // Rototranslation matrices
                        SE3<> GazFrame_T_mipBodyFrame;
                        SE3<> mipBodyFrame_T_Cam;           // NOT USED
                        SE3<> Cam_T_PtammFrame;             // NOT USED
                        SE3<> GazFrame_T_PtammFrame;        // NOT USED

                        // EKF FILTER - DATA FUSION
                        vector<Decimal> filterMeas[9];  
                        vector<Decimal> refMeas[7];     
                       
                        // LOG variables
                        // text log file
                        ofstream uavLogFile_Gazebo;     // log of data from Gazebo
      
                        bool gazLogAct;
                        // Graphical output window
                        DraWin*      _draWin;   
   unsigned int _drawList; 
   bool         dwOn;      
                        /* ------------------------------ */

   /* PRIVATE METHODS OF THE TASK */
   void _firstRun();                        
                        void _ptammFirstState();
                        void _ptammSecondState();
                        void _ptammThirdState();
                        void _ptammFourthState();
                        void _ptammNavigation();
   void _normalRun();
   void _lastRun();
                        /* -------------------------- */
                        /* ---------UTILITIES-------- */
                        inline bool readFromGazebo(Decimal *posX, Decimal *posY, Decimal *posZ,
                                                  Decimal *velX, Decimal *velY, Decimal *velZ,
                                                  Decimal *accX, Decimal *accY, Decimal *accZ,
                                                  Decimal *roll, Decimal *pitch, Decimal *yaw);
                        inline SE3<> computeTransMatrix(Decimal roll, Decimal pitch, Decimal yaw,
                                                        Decimal trans_x, Decimal trans_y, Decimal trans_z );
                        inline void applyTransformToVector(SE3<> transform, Decimal *x, Decimal *y, Decimal *z);
                        inline void logGazMeas();
                        /* -------------------------- */
  public:
   UAVPositioningSystem(ResourcePointers resources,int argc, const char* argv[]);

   ~UAVPositioningSystem();

                        /* MANDATORY METHODS OF MIP TASK */
   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