Fawkes API  Fawkes Development Version
relvelo.h
00001 
00002 /***************************************************************************
00003  *  relvelo.h - A simple velocity model using the relative coordinates and
00004  *              robot velocity
00005  *
00006  *  Created: Tue Oct 04 15:49:23 2005
00007  *  Copyright  2005  Tim Niemueller [www.niemueller.de]
00008  *
00009  ****************************************************************************/
00010                                                                                 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version. A runtime exception applies to
00015  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00016  *
00017  *  This program is distributed in the hope that it will be useful,
00018  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  *  GNU Library General Public License for more details.
00021  *
00022  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00023  */
00024 
00025 #ifndef __FIREVISION_MODELS_VELOCITY_RELATIVE_H_
00026 #define __FIREVISION_MODELS_VELOCITY_RELATIVE_H_
00027 
00028 #include <models/velocity/velocitymodel.h>
00029 #include <models/relative_position/relativepositionmodel.h>
00030 
00031 #include <fvutils/base/types.h>
00032 
00033 // include <utils/kalman_filter/ckalman_filter_2dim.h>
00034 #include <list>
00035 
00036 namespace firevision {
00037 #if 0 /* just to make Emacs auto-indent happy */
00038 }
00039 #endif
00040 
00041 /** Position/time tuple. */
00042 typedef struct {
00043   float   x;    /**< x pos */
00044   float   y;    /**< y pos */
00045   timeval t;    /**< time */
00046 } vel_postime_t;
00047 
00048 /** Velocity/time tuple. */
00049 typedef struct {
00050   float   vx;   /**< vx in m/s */
00051   float   vy;   /**< vy in m/s */
00052   timeval t;    /**< time */
00053 } vel_veltime_t;
00054 
00055 class VelocityFromRelative : public VelocityModel
00056 {
00057  public:
00058   VelocityFromRelative(RelativePositionModel* model, unsigned int max_history_length, unsigned int calc_interval);
00059   virtual ~VelocityFromRelative();
00060 
00061   virtual const char * getName() const;
00062 
00063   virtual void  setRobotPosition(float x, float y, float ori, timeval t);
00064   virtual void  setRobotVelocity(float vel_x, float vel_y, timeval t);
00065   virtual void  setPanTilt(float pan, float tilt);
00066   virtual void  setTime(timeval t);
00067   virtual void  setTimeNow();
00068   virtual void  getTime(long int *sec, long int *usec);
00069 
00070   virtual void  getVelocity(float *vel_x, float *vel_y);
00071 
00072   virtual float getVelocityX();
00073   virtual float getVelocityY();
00074 
00075   virtual void  calc();
00076   virtual void  reset();
00077 
00078   virtual coordsys_type_t getCoordinateSystem();
00079 
00080  private:
00081   RelativePositionModel *relative_pos_model;
00082 
00083   float                  robot_rel_vel_x;
00084   float                  robot_rel_vel_y;
00085   timeval                robot_rel_vel_t;
00086   timeval                vel_last_time;
00087 
00088   timeval                now;
00089   std::list<vel_postime_t *>  ball_history;
00090   std::list<vel_postime_t *>::iterator bh_it;
00091 
00092   int                    diff_sec;
00093   int                    diff_usec;
00094 
00095   float                  f_diff_sec;
00096 
00097   unsigned int           max_history_length;
00098   unsigned int           calc_interval;
00099 
00100   float                  cur_ball_x;
00101   float                  cur_ball_y;
00102   float                  cur_ball_dist;
00103 
00104   // for projection
00105   bool                   last_available;
00106   timeval                last_time;
00107   float                  last_x;
00108   float                  last_y;
00109   float                  proj_x;
00110   float                  proj_y;
00111   float                  last_proj_error_x;
00112   float                  last_proj_error_y;
00113   float                  proj_time_diff_sec;
00114 
00115   float                  diff_x;
00116   float                  diff_y;
00117 
00118   float                  velocity_x;
00119   float                  velocity_y;
00120 
00121   float                  avg_vx_sum;
00122   float                  avg_vy_sum;
00123   float                  avg_vx;
00124   float                  avg_vy;
00125   unsigned int           avg_vx_num;
00126   unsigned int           avg_vy_num;
00127   float                  rx;
00128   float                  ry;
00129   float                  age_factor;
00130 
00131   /*
00132   bool                   kalman_enabled;
00133   float                  var_proc_x;
00134   float                  var_proc_y;
00135   float                  var_meas_x;
00136   float                  var_meas_y;
00137   kalmanFilter2Dim      *kalman_filter;
00138 
00139   void                  applyKalmanFilter();
00140   */
00141 
00142 };
00143 
00144 } // end namespace firevision
00145 
00146 #endif