Fawkes API  Fawkes Development Version
relvelo.cpp
1 
2 /***************************************************************************
3  * relvelo.cpp - Implementation of velocity model based on relative ball
4  * positions and relative robot velocity
5  *
6  * Created: Tue Oct 04 15:54:27 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 <fvmodels/velocity/relvelo.h>
26 #include <utils/system/console_colors.h>
27 #include <utils/time/time.h>
28 
29 #include <cmath>
30 #include <cstdlib>
31 #include <limits>
32 
33 using namespace std;
34 
35 namespace firevision {
36 
37 /** @class VelocityFromRelative <fvmodels/velocity/relvelo.h>
38  * Calculate velocity from relative positions.
39  */
40 
41 /** Constructor.
42  * @param model relative position model
43  * @param max_history_length maximum history length
44  * @param calc_interval calculation interval
45  */
46 VelocityFromRelative::VelocityFromRelative(RelativePositionModel *model,
47  unsigned int max_history_length,
48  unsigned int calc_interval)
49 {
50  this->relative_pos_model = model;
51  this->max_history_length = max_history_length;
52  this->calc_interval = calc_interval;
53 
54  //kalman_enabled = true;
55 
56  robot_rel_vel_x = robot_rel_vel_y = 0.f;
57 
58  velocity_x = velocity_y = 0.f;
59 
60  /*
61  // initial variance for ball pos
62  var_proc_x = 300;
63  var_proc_y = 50;
64  var_meas_x = 300;
65  var_meas_y = 50;
66 
67  // initial variance for ball pos
68  kalman_filter = new kalmanFilter2Dim();
69  CMatrix<float> initialStateVarianceBall(2,2);
70  initialStateVarianceBall[0][0] = var_meas_x;
71  initialStateVarianceBall[1][0] = 0.0;
72  initialStateVarianceBall[0][1] = 0.0;
73  initialStateVarianceBall[1][1] = var_meas_y;
74  kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
75 
76  // process noise for ball pos kf, initial estimates, refined in calc()
77  kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
78  kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
79  */
80 
81  avg_vx_sum = 0.f;
82  avg_vx_num = 0;
83 
84  avg_vy_sum = 0.f;
85  avg_vy_num = 0;
86 
87  ball_history.clear();
88 }
89 
90 /** Destructor. */
91 VelocityFromRelative::~VelocityFromRelative()
92 {
93 }
94 
95 void
96 VelocityFromRelative::setPanTilt(float pan, float tilt)
97 {
98 }
99 
100 void
101 VelocityFromRelative::setRobotPosition(float x, float y, float ori, timeval t)
102 {
103 }
104 
105 void
106 VelocityFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t)
107 {
108  robot_rel_vel_x = rel_vel_x;
109  robot_rel_vel_y = rel_vel_y;
110  robot_rel_vel_t.tv_sec = t.tv_sec;
111  robot_rel_vel_t.tv_usec = t.tv_usec;
112 }
113 
114 void
115 VelocityFromRelative::setTime(timeval t)
116 {
117  now.tv_sec = t.tv_sec;
118  now.tv_usec = t.tv_usec;
119 }
120 
121 void
122 VelocityFromRelative::setTimeNow()
123 {
124  gettimeofday(&now, NULL);
125 }
126 
127 void
128 VelocityFromRelative::getTime(long int *sec, long int *usec)
129 {
130  *sec = now.tv_sec;
131  *usec = now.tv_usec;
132 }
133 
134 void
135 VelocityFromRelative::getVelocity(float *vel_x, float *vel_y)
136 {
137  if (vel_x != NULL) {
138  *vel_x = velocity_x;
139  }
140  if (vel_y != NULL) {
141  *vel_y = velocity_y;
142  }
143 }
144 
145 float
146 VelocityFromRelative::getVelocityX()
147 {
148  return velocity_x;
149 }
150 
151 float
152 VelocityFromRelative::getVelocityY()
153 {
154  return velocity_y;
155 }
156 
157 void
158 VelocityFromRelative::calc()
159 {
160  /*
161  char user_input = toupper( getkey() );
162 
163  if ( ! relative_pos_model->isPosValid() ) {
164  return;
165  }
166 
167  if (user_input == 'P') {
168  cout << "Enter new kalman process variance values (X Y):" << flush;
169  cin >> var_proc_x >> var_proc_y;
170  } else if (user_input == 'M') {
171  cout << "Enter new kalman measurement variance values (X Y):" << flush;
172  cin >> var_meas_x >> var_meas_y;
173  } else if (user_input == 'R') {
174  cout << "Reset" << endl;
175  reset();
176  } else if (user_input == 'C') {
177  cout << "Current kalman measurement variance (X Y) = ("
178  << var_meas_x << " " << var_meas_y << ")" << endl
179  << "Current kalman process variance (X Y) = ("
180  << var_proc_x << " " << var_proc_y << ")" << endl;
181  } else if (user_input == 'K') {
182  kalman_enabled = ! kalman_enabled;
183  if ( kalman_enabled ) {
184  cout << "Kalman filtering enabled" << endl;
185  kalman_filter->reset();
186  } else {
187  cout << "Kalman filtering disabled" << endl;
188  }
189  }
190  */
191 
192  // Gather needed data
193  cur_ball_x = relative_pos_model->get_x();
194  cur_ball_y = relative_pos_model->get_y();
195  cur_ball_dist = relative_pos_model->get_distance();
196 
197  if (isnan(cur_ball_x) || isinf(cur_ball_x) || isnan(cur_ball_y) || isinf(cur_ball_y)
198  || isnan(cur_ball_dist) || isinf(cur_ball_dist)) {
199  // cout << cred << "relative position model returned nan/inf value(s)!" << cnormal << endl;
200  return;
201  }
202 
203  // if we project the last ball position by the velocity we calculated
204  // at that time we can compare this to the current position and estimate
205  // an error from this information
206  if (last_available) {
207  proj_time_diff_sec = fawkes::time_diff_sec(now, last_time);
208  proj_x = last_x + velocity_x * proj_time_diff_sec;
209  proj_y = last_y + velocity_y * proj_time_diff_sec;
210  last_proj_error_x = cur_ball_x - proj_x;
211  last_proj_error_y = cur_ball_y - proj_y;
212  last_available = false;
213  } else {
214  last_proj_error_x = cur_ball_x;
215  last_proj_error_y = cur_ball_x;
216  }
217 
218  // newest entry first
219  vel_postime_t *vpt = (vel_postime_t *)malloc(sizeof(vel_postime_t));
220  ;
221  vpt->x = cur_ball_x;
222  vpt->y = cur_ball_y;
223  vpt->t.tv_sec = now.tv_sec;
224  vpt->t.tv_usec = now.tv_usec;
225  ball_history.push_front(vpt);
226 
227  if (ball_history.size() >= 2) {
228  // we need at least two entries
229  // take the last robot velocity, then find the corresponding ball_pos entry
230  // in the history and an entry about 100ms away to extrapolate the
231  // ball velocity, then correct this by the robot's velocity we got
232 
233  if (fawkes::time_diff_sec(robot_rel_vel_t, vel_last_time) != 0) {
234  // We have a new robot position data, calculate new velocity
235 
236  vel_last_time.tv_sec = robot_rel_vel_t.tv_sec;
237  vel_last_time.tv_usec = robot_rel_vel_t.tv_usec;
238 
239  f_diff_sec = numeric_limits<float>::max();
240  float time_diff;
241  vel_postime_t *young = NULL;
242  vel_postime_t *old = NULL;
243  unsigned int step = 0;
244  for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) {
245  // Find the ball pos history entry closest in time (but still younger) to
246  // the new position data
247  time_diff = fawkes::time_diff_sec((*bh_it)->t, robot_rel_vel_t);
248  if ((time_diff > 0) && (time_diff < f_diff_sec)) {
249  f_diff_sec = time_diff;
250  young = (*bh_it);
251  } else {
252  // Now find second time
253  if (step != calc_interval) {
254  ++step;
255  } else {
256  // Found a second time
257  old = *bh_it;
258  ++bh_it;
259  break;
260  }
261  }
262  }
263 
264  if ((young != NULL) && (old != NULL)) {
265  // we found two valid times
266 
267  diff_x = young->x - old->x;
268  diff_y = young->y - old->y;
269 
270  f_diff_sec = fawkes::time_diff_sec(young->t, old->t);
271 
272  velocity_x = diff_x / f_diff_sec;
273  velocity_y = diff_y / f_diff_sec;
274 
275  //cout << "f_diff_sec=" << f_diff_sec << " vx=" << velocity_x << " vy=" << velocity_y << endl;
276 
277  velocity_x += robot_rel_vel_x;
278  velocity_y += robot_rel_vel_y;
279 
280  velocity_x -= last_proj_error_x * proj_time_diff_sec;
281  velocity_y -= last_proj_error_y * proj_time_diff_sec;
282 
283  //cout << "vx+rx=" << velocity_x << " vy+ry=" << velocity_y << endl;
284 
285  /*
286  cout << endl
287  << "VELOCITY CALCULATION" << endl
288  << " History size : " << ball_history.size() << endl
289  << " Ball position" << endl
290  << " young : (" << young->x << ", " << young->y << ")" << endl
291  << " old : (" << old->x << ", " << old->y << ")" << endl
292  << " difference : " << diff_x << ", " << diff_y << ")" << endl
293  << " Time" << endl
294  << " current : " << young->t.tv_sec << " sec, " << young->t.tv_usec << " usec" << endl
295  << " old : " << old->t.tv_sec << " sec, " << old->t.tv_usec << " usec" << endl
296  << " difference : " << f_diff_sec << " sec" << endl
297  << " Projection" << endl
298  << " proj error : (" << last_proj_error_x << "," << last_proj_error_y << ")" << endl
299  << " Velocity" << endl
300  << " robot : (" << robot_rel_vel_x << ", " << robot_rel_vel_y << ")" << endl
301  << " Ball" << endl
302  << " raw : (" << velocity_x - robot_rel_vel_x << ", " << velocity_y - robot_rel_vel_y << ")" << endl
303  << " corrected : (" << velocity_x << ", " << velocity_y << ")" << endl;
304  */
305 
306  /*
307  if ( kalman_enabled ) {
308  applyKalmanFilter();
309  }
310  */
311 
312  last_x = cur_ball_x;
313  last_y = cur_ball_y;
314  last_time.tv_sec = now.tv_sec;
315  last_time.tv_usec = now.tv_usec;
316  last_available = true;
317 
318  /*
319  cout << " filtered : (" << clightpurple << velocity_x << cnormal
320  << ", " << clightpurple << velocity_y << cnormal << ")" << endl
321  << endl;
322  */
323 
324  // erase old history entries
325  if (bh_it != ball_history.end()) {
326  ball_history.erase(bh_it, ball_history.end());
327  }
328  } else {
329  // cout << "did not find matching young and old record" << endl;
330  velocity_x = 0.f;
331  velocity_y = 0.f;
332  }
333  } else {
334  // we did not get a new robot position, keep old velocities for 2 seconds
335  if (fawkes::time_diff_sec(now, vel_last_time) > 2) {
336  // cout << "did not get new robot position for more than 2 sec, resetting" << endl;
337  velocity_x = 0.f;
338  velocity_y = 0.f;
339  }
340  }
341  } else {
342  // cout << "history too short" << endl;
343  velocity_x = 0.f;
344  velocity_y = 0.f;
345  }
346 
347  if (ball_history.size() > max_history_length) {
348  bh_it = ball_history.begin();
349  for (unsigned int i = 0; i < max_history_length; ++i) {
350  ++bh_it;
351  }
352  ball_history.erase(bh_it, ball_history.end());
353  }
354 }
355 
356 void
357 VelocityFromRelative::reset()
358 {
359  /*
360  if (kalman_enabled) {
361  kalman_filter->reset();
362  }
363  */
364  avg_vx_sum = 0.f;
365  avg_vx_num = 0;
366  avg_vy_sum = 0.f;
367  avg_vy_num = 0;
368  velocity_x = 0.f;
369  velocity_y = 0.f;
370  ball_history.clear();
371 }
372 
373 const char *
374 VelocityFromRelative::getName() const
375 {
376  return "VelocityModel::VelocityFromRelative";
377 }
378 
379 coordsys_type_t
380 VelocityFromRelative::getCoordinateSystem()
381 {
382  return COORDSYS_ROBOT_CART;
383 }
384 
385 /*
386 void
387 VelocityFromRelative::applyKalmanFilter()
388 {
389  /
390  avg_vx_sum += velocity_x;
391  avg_vy_sum += velocity_y;
392 
393  ++avg_vx_num;
394  ++avg_vy_num;
395 
396  avg_vx = avg_vx_sum / avg_vx_num;
397  avg_vy = avg_vy_sum / avg_vy_num;
398 
399  age_factor = (fawkes::time_diff_sec(now, robot_rel_vel_t) + f_diff_sec);
400 
401  rx = (velocity_x - avg_vx) * age_factor;
402  ry = (velocity_y - avg_vy) * age_factor;
403 
404  kalman_filter->setProcessCovariance( rx * rx, ry * ry );
405 
406  rx = (velocity_x - avg_vx) * cur_ball_dist;
407  ry = (velocity_y - avg_vy) * cur_ball_dist;
408 
409  kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
410  /
411 
412  kalman_filter->setProcessCovariance( var_proc_x * cur_ball_dist,
413  var_proc_y * cur_ball_dist);
414  kalman_filter->setMeasurementCovariance( var_meas_x * cur_ball_dist,
415  var_meas_y * cur_ball_dist );
416 
417  kalman_filter->setMeasurementX( velocity_x );
418  kalman_filter->setMeasurementY( velocity_y );
419  kalman_filter->doCalculation();
420 
421  velocity_x = kalman_filter->getStateX();
422  velocity_y = kalman_filter->getStateY();
423 
424  velocity_x = round( velocity_x * 10 ) / 10;
425  velocity_y = round( velocity_y * 10 ) / 10;
426 
427  if (isnan(velocity_x) || isinf(velocity_x) ||
428  isnan(velocity_y) || isinf(velocity_y) ) {
429  reset();
430  }
431 
432 }
433 */
434 
435 } // end namespace firevision
firevision::RelativePositionModel
Relative Position Model Interface.
Definition: relativepositionmodel.h:33
firevision::vel_postime_t
Position/time tuple.
Definition: relvelo.h:39
fawkes::time_diff_sec
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:41
firevision::vel_postime_t::x
float x
x pos
Definition: relvelo.h:40
firevision::vel_postime_t::y
float y
y pos
Definition: relvelo.h:41
firevision::vel_postime_t::t
timeval t
time
Definition: relvelo.h:42