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