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