Fawkes API  Fawkes Development Version
globvelo.cpp
1 
2 /***************************************************************************
3  * globvelo.cpp - Implementation of velocity model based on global
4  * positions
5  *
6  * Created: Mon Sep 05 17:12:48 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 #include <cmath>
26 #include <fvmodels/velocity/globvelo.h>
27 
28 #include <utils/time/time.h>
29 
30 namespace firevision {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 /** @class VelocityFromGlobal <fvmodels/velocity/globvelo.h>
36  * Velocity from global positions.
37  */
38 
39 /** Constructor.
40  * @param model global position model
41  * @param history_length maximum history length
42  * @param calc_interval calculation interval
43  */
45  unsigned int history_length,
46  unsigned int calc_interval)
47 {
48  this->global_pos_model = model;
49  this->history_length = history_length;
50  this->calc_interval = calc_interval;
51 
52  robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
53 
54  velocity_x = velocity_y = 0.f;
55 
56  /*
57  // initial variance for ball pos kf
58  kalman_filter = new kalmanFilter2Dim();
59  CMatrix<float> initialStateVarianceBall(2,2);
60  initialStateVarianceBall[0][0] = 2.00;
61  initialStateVarianceBall[1][0] = 0.00;
62  initialStateVarianceBall[0][1] = 0.00;
63  initialStateVarianceBall[1][1] = 2.00;
64  kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
65 
66  // process noise for ball pos kf
67  CMatrix<float> processVarianceBall(2,2);
68  processVarianceBall[0][0] = 0.50;
69  processVarianceBall[1][0] = 0.00;
70  processVarianceBall[0][1] = 0.00;
71  processVarianceBall[1][1] = 0.50;
72  kalman_filter->setProcessCovariance( processVarianceBall );
73 
74  kalman_filter->setMeasurementCovariance( 0.5, 0.5 );
75  */
76 }
77 
78 
79 /** Destructor. */
81 {
82 }
83 
84 
85 void
86 VelocityFromGlobal::setPanTilt(float pan, float tilt)
87 {
88 }
89 
90 
91 void
92 VelocityFromGlobal::setRobotPosition(float x, float y, float ori, timeval t)
93 {
94  timeval _now;
95  gettimeofday(&_now, 0);
96  robot_pos_x = x;
97  robot_pos_y = y;
98  robot_pos_ori = ori;
99  robot_pos_age = fawkes::time_diff_sec(_now, t);
100 }
101 
102 
103 void
104 VelocityFromGlobal::setRobotVelocity(float vel_x, float vel_y, timeval t)
105 {
106 }
107 
108 
109 void
111 {
112  now.tv_sec = t.tv_sec;
113  now.tv_usec = t.tv_usec;
114 }
115 
116 
117 void
119 {
120  gettimeofday(&now, 0);
121 }
122 
123 
124 void
125 VelocityFromGlobal::getTime(long int *sec, long int *usec)
126 {
127  *sec = now.tv_sec;
128  *usec = now.tv_usec;
129 }
130 
131 
132 void
133 VelocityFromGlobal::getVelocity(float *vel_x, float *vel_y)
134 {
135  if (vel_x != 0) {
136  *vel_x = velocity_x;
137  }
138  if (vel_y != 0) {
139  *vel_y = velocity_y;
140  }
141 }
142 
143 
144 float
146 {
147  return velocity_x;
148 }
149 
150 
151 float
153 {
154  return velocity_y;
155 }
156 
157 
158 
159 void
161 {
162 
163  // Gather needed data
164  current_x = global_pos_model->get_x();
165  current_y = global_pos_model->get_y();
166 
167  last_x.push_back( current_x );
168  last_y.push_back( current_y );
169 
170  last_time.push_back(now);
171 
172  velocity_total_x = 0.f;
173  velocity_total_y = 0.f;
174  velocity_num = 0;
175 
176  if (last_x.size() > calc_interval) {
177  // min of sizes
178  unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size() ;
179  for (unsigned int i = calc_interval; i < m; i += calc_interval) {
180  diff_x = last_x[i] - last_x[i - calc_interval];
181  diff_y = last_y[i] - last_y[i - calc_interval];
182 
183  diff_sec = last_time[i].tv_sec - last_time[i - calc_interval].tv_sec;
184  diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
185  if (diff_usec < 0) {
186  diff_sec -= 1;
187  diff_usec += 1000000;
188  }
189 
190  f_diff_sec = diff_sec + diff_usec / 1000000.f;
191 
192  velocity_total_x += diff_x / f_diff_sec;
193  velocity_total_y += diff_y / f_diff_sec;
194  velocity_num++;
195  }
196  }
197 
198  // Get average velocity
199  velocity_x = velocity_total_x / velocity_num;
200  velocity_y = velocity_total_y / velocity_num;
201 
202  // applyKalmanFilter();
203 
204  while (last_x.size() > history_length) {
205  last_x.erase( last_x.begin() );
206  last_y.erase( last_y.begin() );
207  }
208 
209 }
210 
211 
212 void
214 {
215  // kalman_filter->reset();
216 }
217 
218 
219 const char *
221 {
222  return "VelocityModel::VelocityFromGlobal";
223 }
224 
225 
226 coordsys_type_t
228 {
229  return COORDSYS_WORLD_CART;
230 }
231 
232 
233 /*
234 void
235 VelocityFromGlobal::applyKalmanFilter()
236 {
237  kalman_filter->setMeasurementX( velocity_x );
238  kalman_filter->setMeasurementY( velocity_y );
239  kalman_filter->doCalculation();
240 
241  velocity_x = kalman_filter->getStateX();
242  velocity_y = kalman_filter->getStateY();
243 
244  if (isnan(velocity_x) || isinf(velocity_x)) {
245  kalman_filter->reset();
246  velocity_x = 0.f;
247  }
248 
249  if (isnan(velocity_y) || isinf(velocity_y)) {
250  kalman_filter->reset();
251  velocity_y = 0.f;
252  }
253 
254 }
255 */
256 
257 } // end namespace firevision
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
Definition: globvelo.cpp:86
virtual ~VelocityFromGlobal()
Destructor.
Definition: globvelo.cpp:80
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
Definition: globvelo.cpp:213
virtual const char * getName() const
Get name of velocity model.
Definition: globvelo.cpp:220
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
Definition: globvelo.cpp:133
virtual float getVelocityY()
Get velocity of tracked object in X direction.
Definition: globvelo.cpp:152
virtual void setTime(timeval t)
Set current time.
Definition: globvelo.cpp:110
virtual void setTimeNow()
Get current time from system.
Definition: globvelo.cpp:118
virtual float get_x() const =0
Get global x coordinate of object.
virtual float get_y() const =0
Get global y coordinate of object.
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
Definition: globvelo.cpp:125
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:40
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
Definition: globvelo.cpp:104
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
Definition: globvelo.cpp:92
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Definition: globvelo.cpp:160
Global Position Model Interface.
VelocityFromGlobal(GlobalPositionModel *model, unsigned int history_length, unsigned int calc_interval)
Constructor.
Definition: globvelo.cpp:44
virtual float getVelocityX()
Get velocity of tracked object in X direction.
Definition: globvelo.cpp:145
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Definition: globvelo.cpp:227