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/relative_position/relativepositionmodel.h>
29 #include <fvmodels/velocity/velocitymodel.h>
30 #include <fvutils/base/types.h>
31 
32 // include <utils/kalman_filter/ckalman_filter_2dim.h>
33 #include <list>
34 
35 namespace firevision {
36 
37 /** Position/time tuple. */
38 typedef struct
39 {
40  float x; /**< x pos */
41  float y; /**< y pos */
42  timeval t; /**< time */
43 } vel_postime_t;
44 
45 /** Velocity/time tuple. */
46 typedef struct
47 {
48  float vx; /**< vx in m/s */
49  float vy; /**< vy in m/s */
50  timeval t; /**< time */
52 
54 {
55 public:
57  unsigned int max_history_length,
58  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 } // end namespace firevision
136 
137 #endif
firevision::VelocityFromRelative::reset
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
Definition: relvelo.cpp:357
firevision::VelocityFromRelative::setTimeNow
virtual void setTimeNow()
Definition: relvelo.cpp:122
firevision::VelocityModel
Definition: velocitymodel.h:38
firevision::RelativePositionModel
Definition: relativepositionmodel.h:39
firevision::VelocityFromRelative::VelocityFromRelative
VelocityFromRelative(RelativePositionModel *model, unsigned int max_history_length, unsigned int calc_interval)
Constructor.
Definition: relvelo.cpp:46
firevision::VelocityFromRelative::getVelocityY
virtual float getVelocityY()
Definition: relvelo.cpp:152
firevision::VelocityFromRelative::getName
virtual const char * getName() const
Definition: relvelo.cpp:374
firevision::VelocityFromRelative::setRobotVelocity
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Definition: relvelo.cpp:106
firevision::VelocityFromRelative::getVelocity
virtual void getVelocity(float *vel_x, float *vel_y)
Definition: relvelo.cpp:135
firevision::VelocityFromRelative::getCoordinateSystem
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Definition: relvelo.cpp:380
firevision::VelocityFromRelative::getTime
virtual void getTime(long int *sec, long int *usec)
Definition: relvelo.cpp:128
firevision::VelocityFromRelative::getVelocityX
virtual float getVelocityX()
Definition: relvelo.cpp:146
firevision::VelocityFromRelative::~VelocityFromRelative
virtual ~VelocityFromRelative()
Destructor.
Definition: relvelo.cpp:91
firevision::VelocityFromRelative::setPanTilt
virtual void setPanTilt(float pan, float tilt)
Definition: relvelo.cpp:96
firevision::VelocityFromRelative::setTime
virtual void setTime(timeval t)
Definition: relvelo.cpp:115
firevision::VelocityFromRelative::calc
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Definition: relvelo.cpp:158
firevision::VelocityFromRelative
Definition: relvelo.h:60
firevision::vel_veltime_t
Velocity/time tuple.
Definition: relvelo.h:53
firevision::VelocityFromRelative::setRobotPosition
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Definition: relvelo.cpp:101