Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * relvelo.cpp - Implementation of velocity model based on relative ball 00004 * positions and relative robot velocity 00005 * 00006 * Created: Tue Oct 04 15:54:27 2005 00007 * Copyright 2005 Tim Niemueller [www.niemueller.de] 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. A runtime exception applies to 00015 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Library General Public License for more details. 00021 * 00022 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00023 */ 00024 00025 #include <models/velocity/relvelo.h> 00026 00027 #include <utils/system/console_colors.h> 00028 #include <utils/time/time.h> 00029 00030 #include <cmath> 00031 #include <cstdlib> 00032 00033 using namespace std; 00034 00035 namespace firevision { 00036 #if 0 /* just to make Emacs auto-indent happy */ 00037 } 00038 #endif 00039 00040 /** @class VelocityFromRelative <models/velocity/relvelo.h> 00041 * Calculate velocity from relative positions. 00042 */ 00043 00044 /** Constructor. 00045 * @param model relative position model 00046 * @param max_history_length maximum history length 00047 * @param calc_interval calculation interval 00048 */ 00049 VelocityFromRelative::VelocityFromRelative(RelativePositionModel* model, 00050 unsigned int max_history_length, 00051 unsigned int calc_interval) 00052 { 00053 this->relative_pos_model = model; 00054 this->max_history_length = max_history_length; 00055 this->calc_interval = calc_interval; 00056 00057 //kalman_enabled = true; 00058 00059 robot_rel_vel_x = robot_rel_vel_y = 0.f; 00060 00061 velocity_x = velocity_y = 0.f; 00062 00063 /* 00064 // initial variance for ball pos 00065 var_proc_x = 300; 00066 var_proc_y = 50; 00067 var_meas_x = 300; 00068 var_meas_y = 50; 00069 00070 // initial variance for ball pos 00071 kalman_filter = new kalmanFilter2Dim(); 00072 CMatrix<float> initialStateVarianceBall(2,2); 00073 initialStateVarianceBall[0][0] = var_meas_x; 00074 initialStateVarianceBall[1][0] = 0.0; 00075 initialStateVarianceBall[0][1] = 0.0; 00076 initialStateVarianceBall[1][1] = var_meas_y; 00077 kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 00078 00079 // process noise for ball pos kf, initial estimates, refined in calc() 00080 kalman_filter->setProcessCovariance( var_proc_x, var_proc_y ); 00081 kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y ); 00082 */ 00083 00084 avg_vx_sum = 0.f; 00085 avg_vx_num = 0; 00086 00087 avg_vy_sum = 0.f; 00088 avg_vy_num = 0; 00089 00090 ball_history.clear(); 00091 00092 } 00093 00094 00095 /** Destructor. */ 00096 VelocityFromRelative::~VelocityFromRelative() 00097 { 00098 } 00099 00100 00101 void 00102 VelocityFromRelative::setPanTilt(float pan, float tilt) 00103 { 00104 } 00105 00106 00107 void 00108 VelocityFromRelative::setRobotPosition(float x, float y, float ori, timeval t) 00109 { 00110 } 00111 00112 00113 void 00114 VelocityFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t) 00115 { 00116 robot_rel_vel_x = rel_vel_x; 00117 robot_rel_vel_y = rel_vel_y; 00118 robot_rel_vel_t.tv_sec = t.tv_sec; 00119 robot_rel_vel_t.tv_usec = t.tv_usec; 00120 } 00121 00122 void 00123 VelocityFromRelative::setTime(timeval t) 00124 { 00125 now.tv_sec = t.tv_sec; 00126 now.tv_usec = t.tv_usec; 00127 } 00128 00129 00130 void 00131 VelocityFromRelative::setTimeNow() 00132 { 00133 gettimeofday(&now, NULL); 00134 } 00135 00136 00137 void 00138 VelocityFromRelative::getTime(long int *sec, long int *usec) 00139 { 00140 *sec = now.tv_sec; 00141 *usec = now.tv_usec; 00142 } 00143 00144 00145 void 00146 VelocityFromRelative::getVelocity(float *vel_x, float *vel_y) 00147 { 00148 if (vel_x != NULL) { 00149 *vel_x = velocity_x; 00150 } 00151 if (vel_y != NULL) { 00152 *vel_y = velocity_y; 00153 } 00154 } 00155 00156 00157 float 00158 VelocityFromRelative::getVelocityX() 00159 { 00160 return velocity_x; 00161 } 00162 00163 00164 float 00165 VelocityFromRelative::getVelocityY() 00166 { 00167 return velocity_y; 00168 } 00169 00170 00171 void 00172 VelocityFromRelative::calc() 00173 { 00174 /* 00175 char user_input = toupper( getkey() ); 00176 00177 if ( ! relative_pos_model->isPosValid() ) { 00178 return; 00179 } 00180 00181 if (user_input == 'P') { 00182 cout << "Enter new kalman process variance values (X Y):" << flush; 00183 cin >> var_proc_x >> var_proc_y; 00184 } else if (user_input == 'M') { 00185 cout << "Enter new kalman measurement variance values (X Y):" << flush; 00186 cin >> var_meas_x >> var_meas_y; 00187 } else if (user_input == 'R') { 00188 cout << "Reset" << endl; 00189 reset(); 00190 } else if (user_input == 'C') { 00191 cout << "Current kalman measurement variance (X Y) = (" 00192 << var_meas_x << " " << var_meas_y << ")" << endl 00193 << "Current kalman process variance (X Y) = (" 00194 << var_proc_x << " " << var_proc_y << ")" << endl; 00195 } else if (user_input == 'K') { 00196 kalman_enabled = ! kalman_enabled; 00197 if ( kalman_enabled ) { 00198 cout << "Kalman filtering enabled" << endl; 00199 kalman_filter->reset(); 00200 } else { 00201 cout << "Kalman filtering disabled" << endl; 00202 } 00203 } 00204 */ 00205 00206 // Gather needed data 00207 cur_ball_x = relative_pos_model->get_x(); 00208 cur_ball_y = relative_pos_model->get_y(); 00209 cur_ball_dist = relative_pos_model->get_distance(); 00210 00211 if ( isnan(cur_ball_x) || isinf(cur_ball_x) || 00212 isnan(cur_ball_y) || isinf(cur_ball_y) || 00213 isnan(cur_ball_dist) || isinf(cur_ball_dist) ) { 00214 // cout << cred << "relative position model returned nan/inf value(s)!" << cnormal << endl; 00215 return; 00216 } 00217 00218 // if we project the last ball position by the velocity we calculated 00219 // at that time we can compare this to the current position and estimate 00220 // an error from this information 00221 if (last_available) { 00222 proj_time_diff_sec = fawkes::time_diff_sec(now, last_time); 00223 proj_x = last_x + velocity_x * proj_time_diff_sec; 00224 proj_y = last_y + velocity_y * proj_time_diff_sec; 00225 last_proj_error_x = cur_ball_x - proj_x; 00226 last_proj_error_y = cur_ball_y - proj_y; 00227 last_available = false; 00228 } else { 00229 last_proj_error_x = cur_ball_x; 00230 last_proj_error_y = cur_ball_x; 00231 } 00232 00233 00234 // newest entry first 00235 vel_postime_t *vpt = (vel_postime_t *)malloc(sizeof(vel_postime_t));; 00236 vpt->x = cur_ball_x; 00237 vpt->y = cur_ball_y; 00238 vpt->t.tv_sec = now.tv_sec; 00239 vpt->t.tv_usec = now.tv_usec; 00240 ball_history.push_front( vpt ); 00241 00242 00243 if (ball_history.size() >= 2) { 00244 00245 // we need at least two entries 00246 // take the last robot velocity, then find the corresponding ball_pos entry 00247 // in the history and an entry about 100ms away to extrapolate the 00248 // ball velocity, then correct this by the robot's velocity we got 00249 00250 if ( fawkes::time_diff_sec(robot_rel_vel_t, vel_last_time) != 0 ) { 00251 // We have a new robot position data, calculate new velocity 00252 00253 vel_last_time.tv_sec = robot_rel_vel_t.tv_sec; 00254 vel_last_time.tv_usec = robot_rel_vel_t.tv_usec; 00255 00256 f_diff_sec = HUGE; 00257 float time_diff; 00258 vel_postime_t *young = NULL; 00259 vel_postime_t *old = NULL; 00260 unsigned int step = 0; 00261 for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) { 00262 // Find the ball pos history entry closest in time (but still younger) to 00263 // the new position data 00264 time_diff = fawkes::time_diff_sec((*bh_it)->t, robot_rel_vel_t); 00265 if ( (time_diff > 0) && (time_diff < f_diff_sec) ) { 00266 f_diff_sec = time_diff; 00267 young = (*bh_it); 00268 } else { 00269 // Now find second time 00270 if (step != calc_interval) { 00271 ++step; 00272 } else { 00273 // Found a second time 00274 old = *bh_it; 00275 ++bh_it; 00276 break; 00277 } 00278 } 00279 } 00280 00281 if ((young != NULL) && (old != NULL)) { 00282 // we found two valid times 00283 00284 diff_x = young->x - old->x; 00285 diff_y = young->y - old->y; 00286 00287 f_diff_sec = fawkes::time_diff_sec(young->t, old->t); 00288 00289 velocity_x = diff_x / f_diff_sec; 00290 velocity_y = diff_y / f_diff_sec; 00291 00292 //cout << "f_diff_sec=" << f_diff_sec << " vx=" << velocity_x << " vy=" << velocity_y << endl; 00293 00294 velocity_x += robot_rel_vel_x; 00295 velocity_y += robot_rel_vel_y; 00296 00297 velocity_x -= last_proj_error_x * proj_time_diff_sec; 00298 velocity_y -= last_proj_error_y * proj_time_diff_sec; 00299 00300 //cout << "vx+rx=" << velocity_x << " vy+ry=" << velocity_y << endl; 00301 00302 /* 00303 cout << endl 00304 << "VELOCITY CALCULATION" << endl 00305 << " History size : " << ball_history.size() << endl 00306 << " Ball position" << endl 00307 << " young : (" << young->x << ", " << young->y << ")" << endl 00308 << " old : (" << old->x << ", " << old->y << ")" << endl 00309 << " difference : " << diff_x << ", " << diff_y << ")" << endl 00310 << " Time" << endl 00311 << " current : " << young->t.tv_sec << " sec, " << young->t.tv_usec << " usec" << endl 00312 << " old : " << old->t.tv_sec << " sec, " << old->t.tv_usec << " usec" << endl 00313 << " difference : " << f_diff_sec << " sec" << endl 00314 << " Projection" << endl 00315 << " proj error : (" << last_proj_error_x << "," << last_proj_error_y << ")" << endl 00316 << " Velocity" << endl 00317 << " robot : (" << robot_rel_vel_x << ", " << robot_rel_vel_y << ")" << endl 00318 << " Ball" << endl 00319 << " raw : (" << velocity_x - robot_rel_vel_x << ", " << velocity_y - robot_rel_vel_y << ")" << endl 00320 << " corrected : (" << velocity_x << ", " << velocity_y << ")" << endl; 00321 */ 00322 00323 /* 00324 if ( kalman_enabled ) { 00325 applyKalmanFilter(); 00326 } 00327 */ 00328 00329 last_x = cur_ball_x; 00330 last_y = cur_ball_y; 00331 last_time.tv_sec = now.tv_sec; 00332 last_time.tv_usec = now.tv_usec; 00333 last_available = true; 00334 00335 /* 00336 cout << " filtered : (" << clightpurple << velocity_x << cnormal 00337 << ", " << clightpurple << velocity_y << cnormal << ")" << endl 00338 << endl; 00339 */ 00340 00341 // erase old history entries 00342 if (bh_it != ball_history.end()) { 00343 ball_history.erase(bh_it, ball_history.end()); 00344 } 00345 } else { 00346 // cout << "did not find matching young and old record" << endl; 00347 velocity_x = 0.f; 00348 velocity_y = 0.f; 00349 } 00350 } else { 00351 // we did not get a new robot position, keep old velocities for 2 seconds 00352 if (fawkes::time_diff_sec(now, vel_last_time) > 2) { 00353 // cout << "did not get new robot position for more than 2 sec, resetting" << endl; 00354 velocity_x = 0.f; 00355 velocity_y = 0.f; 00356 } 00357 } 00358 } else { 00359 // cout << "history too short" << endl; 00360 velocity_x = 0.f; 00361 velocity_y = 0.f; 00362 } 00363 00364 if (ball_history.size() > max_history_length) { 00365 bh_it = ball_history.begin(); 00366 for (unsigned int i = 0; i < max_history_length; ++i) { 00367 ++bh_it; 00368 } 00369 ball_history.erase(bh_it, ball_history.end()); 00370 } 00371 00372 } 00373 00374 00375 void 00376 VelocityFromRelative::reset() 00377 { 00378 /* 00379 if (kalman_enabled) { 00380 kalman_filter->reset(); 00381 } 00382 */ 00383 avg_vx_sum = 0.f; 00384 avg_vx_num = 0; 00385 avg_vy_sum = 0.f; 00386 avg_vy_num = 0; 00387 velocity_x = 0.f; 00388 velocity_y = 0.f; 00389 ball_history.clear(); 00390 } 00391 00392 00393 const char * 00394 VelocityFromRelative::getName() const 00395 { 00396 return "VelocityModel::VelocityFromRelative"; 00397 } 00398 00399 00400 coordsys_type_t 00401 VelocityFromRelative::getCoordinateSystem() 00402 { 00403 return COORDSYS_ROBOT_CART; 00404 } 00405 00406 /* 00407 void 00408 VelocityFromRelative::applyKalmanFilter() 00409 { 00410 / 00411 avg_vx_sum += velocity_x; 00412 avg_vy_sum += velocity_y; 00413 00414 ++avg_vx_num; 00415 ++avg_vy_num; 00416 00417 avg_vx = avg_vx_sum / avg_vx_num; 00418 avg_vy = avg_vy_sum / avg_vy_num; 00419 00420 age_factor = (fawkes::time_diff_sec(now, robot_rel_vel_t) + f_diff_sec); 00421 00422 rx = (velocity_x - avg_vx) * age_factor; 00423 ry = (velocity_y - avg_vy) * age_factor; 00424 00425 kalman_filter->setProcessCovariance( rx * rx, ry * ry ); 00426 00427 rx = (velocity_x - avg_vx) * cur_ball_dist; 00428 ry = (velocity_y - avg_vy) * cur_ball_dist; 00429 00430 kalman_filter->setMeasurementCovariance( rx * rx, ry * ry ); 00431 / 00432 00433 kalman_filter->setProcessCovariance( var_proc_x * cur_ball_dist, 00434 var_proc_y * cur_ball_dist); 00435 kalman_filter->setMeasurementCovariance( var_meas_x * cur_ball_dist, 00436 var_meas_y * cur_ball_dist ); 00437 00438 kalman_filter->setMeasurementX( velocity_x ); 00439 kalman_filter->setMeasurementY( velocity_y ); 00440 kalman_filter->doCalculation(); 00441 00442 velocity_x = kalman_filter->getStateX(); 00443 velocity_y = kalman_filter->getStateY(); 00444 00445 velocity_x = round( velocity_x * 10 ) / 10; 00446 velocity_y = round( velocity_y * 10 ) / 10; 00447 00448 if (isnan(velocity_x) || isinf(velocity_x) || 00449 isnan(velocity_y) || isinf(velocity_y) ) { 00450 reset(); 00451 } 00452 00453 } 00454 */ 00455 00456 } // end namespace firevision