00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
00037 }
00038 #endif
00039
00040
00041
00042
00043
00044
00045
00046
00047
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
00058
00059 robot_rel_vel_x = robot_rel_vel_y = 0.f;
00060
00061 velocity_x = velocity_y = 0.f;
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
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
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
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
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
00215 return;
00216 }
00217
00218
00219
00220
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
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
00246
00247
00248
00249
00250 if ( fawkes::time_diff_sec(robot_rel_vel_t, vel_last_time) != 0 ) {
00251
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
00263
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
00270 if (step != calc_interval) {
00271 ++step;
00272 } else {
00273
00274 old = *bh_it;
00275 ++bh_it;
00276 break;
00277 }
00278 }
00279 }
00280
00281 if ((young != NULL) && (old != NULL)) {
00282
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
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
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
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
00337
00338
00339
00340
00341
00342 if (bh_it != ball_history.end()) {
00343 ball_history.erase(bh_it, ball_history.end());
00344 }
00345 } else {
00346
00347 velocity_x = 0.f;
00348 velocity_y = 0.f;
00349 }
00350 } else {
00351
00352 if (fawkes::time_diff_sec(now, vel_last_time) > 2) {
00353
00354 velocity_x = 0.f;
00355 velocity_y = 0.f;
00356 }
00357 }
00358 } else {
00359
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
00380
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
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456 }