00001
00002 /***************************************************************************
00003  *  globvelo.h - A simple velocity model using the global coordinates
00004  *
00005  *  Generated: Mon Sep 05 17:06:54 2005
00006  *  Copyright  2005  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023
00024 #ifndef __FIREVISION_MODELS_VELOCITY_GLOBAL_H_
00025 #define __FIREVISION_MODELS_VELOCITY_GLOBAL_H_
00026 
00027 #include <models/velocity/velocitymodel.h>
00028 #include <models/global_position/globalpositionmodel.h>
00029
00030 #include <sys/time.h>
00031 #include <vector>
00032
00033 // include <utils/kalman_filter/ckalman_filter_2dim.h>
00034
00035
00036 class VelocityFromGlobal : public VelocityModel
00037 {
00038  public:
00039   VelocityFromGlobal(GlobalPositionModel* model, unsigned int history_length, unsigned int calc_interval);
00040   virtual ~VelocityFromGlobal();
00041
00042   virtual const char * getName() const;
00043
00044   virtual void  setRobotPosition(float x, float y, float ori, timeval t);
00045   virtual void  setRobotVelocity(float vel_x, float vel_y, timeval t);
00046   virtual void  setPanTilt(float pan, float tilt);
00047   virtual void  setTime(timeval t);
00048   virtual void  setTimeNow();
00049   virtual void  getTime(long int *sec, long int *usec);
00050
00051   virtual void  getVelocity(float *vel_x, float *vel_y);
00052
00053   virtual float getVelocityX();
00054   virtual float getVelocityY();
00055
00056   virtual void  calc();
00057   virtual void  reset();
00058
00059   virtual coordsys_type_t getCoordinateSystem();
00060
00061  private:
00062   GlobalPositionModel   *global_pos_model;
00063
00064   float                  robot_pos_x;
00065   float                  robot_pos_y;
00066   float                  robot_pos_ori;
00067   float                  robot_pos_age;
00068
00069   timeval                now;
00070   std::vector<timeval>        last_time;
00071
00072   unsigned int           history_length;
00073   unsigned int           calc_interval;
00074
00075   int                    diff_sec;
00076   int                    diff_usec;
00077
00078   float                  f_diff_sec;
00079
00080
00081   std::vector<float>     last_x;
00082   std::vector<float>     last_y;
00083
00084   float                  current_x;
00085   float                  current_y;
00086
00087   float                  diff_x;
00088   float                  diff_y;
00089
00090   float                  velocity_total_x;
00091   float                  velocity_total_y;
00092   float                  velocity_num;
00093
00094   float                  velocity_x;
00095   float                  velocity_y;
00096
00097   /*
00098   kalmanFilter2Dim      *kalman_filter;
00099 
00100   void                  applyKalmanFilter();
00101   */
00102
00103 };
00104
00105 #endif