Fawkes API  Fawkes Development Version
relvelo.h
1 
2 /***************************************************************************
3  * relvelo.h - A simple velocity model using the relative coordinates and
4  * robot velocity
5  *
6  * Created: Tue Oct 04 15:49:23 2005
7  * Copyright 2005 Tim Niemueller [www.niemueller.de]
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version. A runtime exception applies to
15  * this software (see LICENSE.GPL_WRE file mentioned below for details).
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU Library General Public License for more details.
21  *
22  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23  */
24 
25 #ifndef __FIREVISION_MODELS_VELOCITY_RELATIVE_H_
26 #define __FIREVISION_MODELS_VELOCITY_RELATIVE_H_
27 
28 #include <fvmodels/velocity/velocitymodel.h>
29 #include <fvmodels/relative_position/relativepositionmodel.h>
30 
31 #include <fvutils/base/types.h>
32 
33 // include <utils/kalman_filter/ckalman_filter_2dim.h>
34 #include <list>
35 
36 namespace firevision {
37 #if 0 /* just to make Emacs auto-indent happy */
38 }
39 #endif
40 
41 /** Position/time tuple. */
42 typedef struct {
43  float x; /**< x pos */
44  float y; /**< y pos */
45  timeval t; /**< time */
47 
48 /** Velocity/time tuple. */
49 typedef struct {
50  float vx; /**< vx in m/s */
51  float vy; /**< vy in m/s */
52  timeval t; /**< time */
54 
56 {
57  public:
58  VelocityFromRelative(RelativePositionModel* model, unsigned int max_history_length, unsigned int calc_interval);
59  virtual ~VelocityFromRelative();
60 
61  virtual const char * getName() const;
62 
63  virtual void setRobotPosition(float x, float y, float ori, timeval t);
64  virtual void setRobotVelocity(float vel_x, float vel_y, timeval t);
65  virtual void setPanTilt(float pan, float tilt);
66  virtual void setTime(timeval t);
67  virtual void setTimeNow();
68  virtual void getTime(long int *sec, long int *usec);
69 
70  virtual void getVelocity(float *vel_x, float *vel_y);
71 
72  virtual float getVelocityX();
73  virtual float getVelocityY();
74 
75  virtual void calc();
76  virtual void reset();
77 
78  virtual coordsys_type_t getCoordinateSystem();
79 
80  private:
81  RelativePositionModel *relative_pos_model;
82 
83  float robot_rel_vel_x;
84  float robot_rel_vel_y;
85  timeval robot_rel_vel_t;
86  timeval vel_last_time;
87 
88  timeval now;
89  std::list<vel_postime_t *> ball_history;
90  std::list<vel_postime_t *>::iterator bh_it;
91 
92  float f_diff_sec;
93 
94  unsigned int max_history_length;
95  unsigned int calc_interval;
96 
97  float cur_ball_x;
98  float cur_ball_y;
99  float cur_ball_dist;
100 
101  // for projection
102  bool last_available;
103  timeval last_time;
104  float last_x;
105  float last_y;
106  float proj_x;
107  float proj_y;
108  float last_proj_error_x;
109  float last_proj_error_y;
110  float proj_time_diff_sec;
111 
112  float diff_x;
113  float diff_y;
114 
115  float velocity_x;
116  float velocity_y;
117 
118  float avg_vx_sum;
119  float avg_vy_sum;
120  unsigned int avg_vx_num;
121  unsigned int avg_vy_num;
122 
123  /*
124  bool kalman_enabled;
125  float var_proc_x;
126  float var_proc_y;
127  float var_meas_x;
128  float var_meas_y;
129  kalmanFilter2Dim *kalman_filter;
130 
131  void applyKalmanFilter();
132  */
133 
134 };
135 
136 } // end namespace firevision
137 
138 #endif
float vx
vx in m/s
Definition: relvelo.h:50
Calculate velocity from relative positions.
Definition: relvelo.h:55
virtual float getVelocityX()
Get velocity of tracked object in X direction.
Definition: relvelo.cpp:158
Position/time tuple.
Definition: relvelo.h:42
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
Definition: relvelo.cpp:146
virtual const char * getName() const
Get name of velocity model.
Definition: relvelo.cpp:394
virtual void setTime(timeval t)
Set current time.
Definition: relvelo.cpp:123
Velocity model interface.
Definition: velocitymodel.h:35
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
Definition: relvelo.cpp:108
virtual float getVelocityY()
Get velocity of tracked object in X direction.
Definition: relvelo.cpp:165
virtual void setTimeNow()
Get current time from system.
Definition: relvelo.cpp:131
Relative Position Model Interface.
VelocityFromRelative(RelativePositionModel *model, unsigned int max_history_length, unsigned int calc_interval)
Constructor.
Definition: relvelo.cpp:49
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
Definition: relvelo.cpp:138
float vy
vy in m/s
Definition: relvelo.h:51
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
Definition: relvelo.cpp:102
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Definition: relvelo.cpp:401
Velocity/time tuple.
Definition: relvelo.h:49
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
Definition: relvelo.cpp:376
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Definition: relvelo.cpp:172
virtual ~VelocityFromRelative()
Destructor.
Definition: relvelo.cpp:96
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
Definition: relvelo.cpp:114