Fawkes API Fawkes Development Version
|
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 <models/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 <models/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