globvelo.h

00001 
00002 /***************************************************************************
00003  *  globvelo.h - A simple velocity model using the global coordinates
00004  *
00005  *  Created: 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 namespace firevision {
00036 #if 0 /* just to make Emacs auto-indent happy */
00037 }
00038 #endif
00039 
00040 class VelocityFromGlobal : public VelocityModel
00041 {
00042  public:
00043   VelocityFromGlobal(GlobalPositionModel* model, unsigned int history_length, unsigned int calc_interval);
00044   virtual ~VelocityFromGlobal();
00045 
00046   virtual const char * getName() const;
00047 
00048   virtual void  setRobotPosition(float x, float y, float ori, timeval t);
00049   virtual void  setRobotVelocity(float vel_x, float vel_y, timeval t);
00050   virtual void  setPanTilt(float pan, float tilt);
00051   virtual void  setTime(timeval t);
00052   virtual void  setTimeNow();
00053   virtual void  getTime(long int *sec, long int *usec);
00054 
00055   virtual void  getVelocity(float *vel_x, float *vel_y);
00056 
00057   virtual float getVelocityX();
00058   virtual float getVelocityY();
00059 
00060   virtual void  calc();
00061   virtual void  reset();
00062 
00063   virtual coordsys_type_t getCoordinateSystem();
00064 
00065  private:
00066   GlobalPositionModel   *global_pos_model;
00067 
00068   float                  robot_pos_x;
00069   float                  robot_pos_y;
00070   float                  robot_pos_ori;
00071   float                  robot_pos_age;
00072 
00073   timeval                now;
00074   std::vector<timeval>        last_time;
00075 
00076   unsigned int           history_length;
00077   unsigned int           calc_interval;
00078 
00079   int                    diff_sec;
00080   int                    diff_usec;
00081 
00082   float                  f_diff_sec;
00083 
00084 
00085   std::vector<float>     last_x;
00086   std::vector<float>     last_y;
00087 
00088   float                  current_x;
00089   float                  current_y;
00090 
00091   float                  diff_x;
00092   float                  diff_y;
00093 
00094   float                  velocity_total_x;
00095   float                  velocity_total_y;
00096   float                  velocity_num;
00097 
00098   float                  velocity_x;
00099   float                  velocity_y;
00100 
00101   /*
00102   kalmanFilter2Dim      *kalman_filter;
00103 
00104   void                  applyKalmanFilter();
00105   */
00106 
00107 };
00108 
00109 } // end namespace firevision
00110 
00111 #endif