00001
00002 /***************************************************************************
00003  *  globvelo.cpp - Implementation of velocity model based on global
00004  *                 positions
00005  *
00006  *  Generated: Mon Sep 05 17:12:48 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 <cmath>
00026 #include <models/velocity/globvelo.h>
00027
00028 #include <utils/time/time.h>
00029 
00030 /** @class VelocityFromGlobal <models/velocity/globvelo.h>
00031  * Velocity from global positions.
00032  */
00033 
00034 /** Constructor.
00035  * @param model global position model
00036  * @param history_length maximum history length
00037  * @param calc_interval calculation interval
00038  */
00039 VelocityFromGlobal::VelocityFromGlobal(GlobalPositionModel* model,
00040                                        unsigned int history_length,
00041                                        unsigned int calc_interval)
00042 {
00043   this->global_pos_model = model;
00044   this->history_length   = history_length;
00045   this->calc_interval    = calc_interval;
00046
00047   robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
00048
00049   velocity_x = velocity_y = 0.f;
00050
00051   /*
00052   // initial variance for ball pos kf
00053   kalman_filter = new kalmanFilter2Dim();
00054   CMatrix<float> initialStateVarianceBall(2,2);
00055   initialStateVarianceBall[0][0] = 2.00;
00056   initialStateVarianceBall[1][0] = 0.00;
00057   initialStateVarianceBall[0][1] = 0.00;
00058   initialStateVarianceBall[1][1] = 2.00;
00059   kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 
00060 
00061   // process noise for ball pos kf
00062   CMatrix<float> processVarianceBall(2,2);
00063   processVarianceBall[0][0] = 0.50;
00064   processVarianceBall[1][0] = 0.00;
00065   processVarianceBall[0][1] = 0.00;
00066   processVarianceBall[1][1] = 0.50;
00067   kalman_filter->setProcessCovariance( processVarianceBall );
00068 
00069   kalman_filter->setMeasurementCovariance( 0.5, 0.5 );
00070   */
00071 }
00072
00073 
00074 /** Destructor. */
00075 VelocityFromGlobal::~VelocityFromGlobal()
00076 {
00077 }
00078
00079
00080 void
00081 VelocityFromGlobal::setPanTilt(float pan, float tilt)
00082 {
00083 }
00084
00085
00086 void
00087 VelocityFromGlobal::setRobotPosition(float x, float y, float ori, timeval t)
00088 {
00089   timeval _now;
00090   gettimeofday(&_now, NULL);
00091   robot_pos_x   = x;
00092   robot_pos_y   = y;
00093   robot_pos_ori = ori;
00094   robot_pos_age = fawkes::time_diff_sec(_now, t);
00095 }
00096
00097
00098 void
00099 VelocityFromGlobal::setRobotVelocity(float vel_x, float vel_y, timeval t)
00100 {
00101 }
00102
00103
00104 void
00105 VelocityFromGlobal::setTime(timeval t)
00106 {
00107   now.tv_sec  = t.tv_sec;
00108   now.tv_usec = t.tv_usec;
00109 }
00110
00111
00112 void
00113 VelocityFromGlobal::setTimeNow()
00114 {
00115   gettimeofday(&now, NULL);
00116 }
00117
00118
00119 void
00120 VelocityFromGlobal::getTime(long int *sec, long int *usec)
00121 {
00122   *sec  = now.tv_sec;
00123   *usec = now.tv_usec;
00124 }
00125
00126
00127 void
00128 VelocityFromGlobal::getVelocity(float *vel_x, float *vel_y)
00129 {
00130   if (vel_x != NULL) {
00131     *vel_x = velocity_x;
00132   }
00133   if (vel_y != NULL) {
00134     *vel_y = velocity_y;
00135   }
00136 }
00137
00138
00139 float
00140 VelocityFromGlobal::getVelocityX()
00141 {
00142   return velocity_x;
00143 }
00144
00145
00146 float
00147 VelocityFromGlobal::getVelocityY()
00148 {
00149   return velocity_y;
00150 }
00151
00152
00153
00154 void
00155 VelocityFromGlobal::calc()
00156 {
00157
00158   // Gather needed data
00159   current_x = global_pos_model->get_x();
00160   current_y = global_pos_model->get_y();
00161
00162   last_x.push_back( current_x );
00163   last_y.push_back( current_y );
00164
00165   last_time.push_back(now);
00166
00167   velocity_total_x = 0.f;
00168   velocity_total_y = 0.f;
00169   velocity_num     = 0;
00170
00171   if (last_x.size() > calc_interval) {
00172     // min of sizes
00173     unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size() ;
00174     for (unsigned int i = calc_interval; i < m; i += calc_interval) {
00175       diff_x = last_x[i] - last_x[i - calc_interval];
00176       diff_y = last_y[i] - last_y[i - calc_interval];
00177
00178       diff_sec  = last_time[i].tv_sec  - last_time[i - calc_interval].tv_sec;
00179       diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
00180       if (diff_usec < 0) {
00181         diff_sec  -= 1;
00182         diff_usec += 1000000;
00183       }
00184
00185       f_diff_sec = diff_sec + diff_usec / 1000000.f;
00186
00187       velocity_total_x += diff_x / f_diff_sec;
00188       velocity_total_y += diff_y / f_diff_sec;
00189       velocity_num++;
00190     }
00191   }
00192
00193   // Get average velocity
00194   velocity_x = velocity_total_x / velocity_num;
00195   velocity_y = velocity_total_y / velocity_num;
00196
00197   // applyKalmanFilter();
00198
00199   while (last_x.size() > history_length) {
00200     last_x.erase( last_x.begin() );
00201     last_y.erase( last_y.begin() );
00202   }
00203
00204 }
00205
00206
00207 void
00208 VelocityFromGlobal::reset()
00209 {
00210   // kalman_filter->reset();
00211 }
00212
00213
00214 const char *
00215 VelocityFromGlobal::getName() const
00216 {
00217   return "VelocityModel::VelocityFromGlobal";
00218 }
00219
00220
00221 coordsys_type_t
00222 VelocityFromGlobal::getCoordinateSystem()
00223 {
00224   return COORDSYS_WORLD_CART;
00225 }
00226
00227
00228 /*
00229 void
00230 VelocityFromGlobal::applyKalmanFilter()
00231 {
00232   kalman_filter->setMeasurementX( velocity_x );
00233   kalman_filter->setMeasurementY( velocity_y );
00234   kalman_filter->doCalculation();
00235 
00236   velocity_x = kalman_filter->getStateX();
00237   velocity_y = kalman_filter->getStateY();
00238 
00239   if (isnan(velocity_x) || isinf(velocity_x)) {
00240     kalman_filter->reset();
00241     velocity_x = 0.f;
00242   }
00243 
00244   if (isnan(velocity_y) || isinf(velocity_y)) {
00245     kalman_filter->reset();
00246     velocity_y = 0.f;
00247   }
00248 
00249 }
00250 */