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