00001
00002 /***************************************************************************
00003  *  relvelo.h - A simple velocity model using the relative coordinates and
00004  *              robot velocity
00005  *
00006  *  Generated: 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 /** Position/time tuple. */
00037 typedef struct {
00038   float   x;    /**< x pos */
00039   float   y;    /**< y pos */
00040   timeval t;    /**< time */
00041 } vel_postime_t;
00042 
00043 /** Velocity/time tuple. */
00044 typedef struct {
00045   float   vx;   /**< vx in m/s */
00046   float   vy;   /**< vy in m/s */
00047   timeval t;    /**< time */
00048 } vel_veltime_t;
00049
00050 class VelocityFromRelative : public VelocityModel
00051 {
00052  public:
00053   VelocityFromRelative(RelativePositionModel* model, unsigned int max_history_length, unsigned int calc_interval);
00054   virtual ~VelocityFromRelative();
00055
00056   virtual const char * getName() const;
00057
00058   virtual void  setRobotPosition(float x, float y, float ori, timeval t);
00059   virtual void  setRobotVelocity(float vel_x, float vel_y, timeval t);
00060   virtual void  setPanTilt(float pan, float tilt);
00061   virtual void  setTime(timeval t);
00062   virtual void  setTimeNow();
00063   virtual void  getTime(long int *sec, long int *usec);
00064
00065   virtual void  getVelocity(float *vel_x, float *vel_y);
00066
00067   virtual float getVelocityX();
00068   virtual float getVelocityY();
00069
00070   virtual void  calc();
00071   virtual void  reset();
00072
00073   virtual coordsys_type_t getCoordinateSystem();
00074
00075  private:
00076   RelativePositionModel *relative_pos_model;
00077
00078   float                  robot_rel_vel_x;
00079   float                  robot_rel_vel_y;
00080   timeval                robot_rel_vel_t;
00081   timeval                vel_last_time;
00082
00083   timeval                now;
00084   std::list<vel_postime_t *>  ball_history;
00085   std::list<vel_postime_t *>::iterator bh_it;
00086
00087   int                    diff_sec;
00088   int                    diff_usec;
00089
00090   float                  f_diff_sec;
00091
00092   unsigned int           max_history_length;
00093   unsigned int           calc_interval;
00094
00095   float                  cur_ball_x;
00096   float                  cur_ball_y;
00097   float                  cur_ball_dist;
00098
00099   // for projection
00100   bool                   last_available;
00101   timeval                last_time;
00102   float                  last_x;
00103   float                  last_y;
00104   float                  proj_x;
00105   float                  proj_y;
00106   float                  last_proj_error_x;
00107   float                  last_proj_error_y;
00108   float                  proj_time_diff_sec;
00109
00110   float                  diff_x;
00111   float                  diff_y;
00112
00113   float                  velocity_x;
00114   float                  velocity_y;
00115
00116   float                  avg_vx_sum;
00117   float                  avg_vy_sum;
00118   float                  avg_vx;
00119   float                  avg_vy;
00120   unsigned int           avg_vx_num;
00121   unsigned int           avg_vy_num;
00122   float                  rx;
00123   float                  ry;
00124   float                  age_factor;
00125
00126   /*
00127   bool                   kalman_enabled;
00128   float                  var_proc_x;
00129   float                  var_proc_y;
00130   float                  var_meas_x;
00131   float                  var_meas_y;
00132   kalmanFilter2Dim      *kalman_filter;
00133 
00134   void                  applyKalmanFilter();
00135   */
00136
00137 };
00138
00139 #endif