Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * globfromrel.cpp - Implementation of velocity model based on relative 00004 * ball positions and relative robot velocity 00005 * 00006 * Created: Fri Oct 21 11:19:03 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 <core/exception.h> 00026 #include <cmath> 00027 #include <models/velocity/globfromrel.h> 00028 #include <utils/time/time.h> 00029 00030 // #include "utils/system/console_colors.h" 00031 // #include "utils/system/time.h" 00032 00033 using namespace std; 00034 using namespace fawkes; 00035 00036 namespace firevision { 00037 #if 0 /* just to make Emacs auto-indent happy */ 00038 } 00039 #endif 00040 00041 /** @class VelocityGlobalFromRelative <models/velocity/globfromrel.h> 00042 * Global velocity from relative velocities. 00043 */ 00044 00045 /** Destructor. 00046 * @param rel_velo_model relative velocity model 00047 * @param rel_pos_model relative position model 00048 */ 00049 VelocityGlobalFromRelative::VelocityGlobalFromRelative(VelocityModel *rel_velo_model, 00050 RelativePositionModel *rel_pos_model) 00051 { 00052 this->relative_velocity = rel_velo_model; 00053 this->relative_position = rel_pos_model; 00054 00055 if ( rel_velo_model->getCoordinateSystem() != COORDSYS_ROBOT_CART ) { 00056 /* 00057 cout << cblue << "VelocityGlobalFromRelative::Constructor: " << cred 00058 << "Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!" 00059 << cnormal << endl; 00060 */ 00061 throw Exception("Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!"); 00062 } 00063 00064 robot_ori = robot_poseage = 0.f; 00065 00066 velocity_x = velocity_y = 0.f; 00067 00068 /* 00069 // initial variance for ball pos kf 00070 kalman_filter = new kalmanFilter2Dim(); 00071 CMatrix<float> initialStateVarianceBall(2,2); 00072 initialStateVarianceBall[0][0] = 5.00; 00073 initialStateVarianceBall[1][0] = 0.00; 00074 initialStateVarianceBall[0][1] = 0.00; 00075 initialStateVarianceBall[1][1] = 5.00; 00076 kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 00077 00078 // process noise for ball pos kf, initial estimates, refined in calc() 00079 kalman_filter->setProcessCovariance( 1.f, 1.f ); 00080 kalman_filter->setMeasurementCovariance( 4.f, 4.f ); 00081 00082 avg_vx_sum = 0.f; 00083 avg_vx_num = 0; 00084 00085 avg_vy_sum = 0.f; 00086 avg_vy_num = 0; 00087 */ 00088 } 00089 00090 00091 /** Destructor. */ 00092 VelocityGlobalFromRelative::~VelocityGlobalFromRelative() 00093 { 00094 } 00095 00096 00097 void 00098 VelocityGlobalFromRelative::setPanTilt(float pan, float tilt) 00099 { 00100 } 00101 00102 00103 void 00104 VelocityGlobalFromRelative::setRobotPosition(float x, float y, float ori, timeval t) 00105 { 00106 timeval now; 00107 gettimeofday(&now, 0); 00108 robot_ori = ori; 00109 robot_poseage = time_diff_sec(now, t); 00110 } 00111 00112 00113 void 00114 VelocityGlobalFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t) 00115 { 00116 } 00117 00118 void 00119 VelocityGlobalFromRelative::setTime(timeval t) 00120 { 00121 } 00122 00123 00124 void 00125 VelocityGlobalFromRelative::setTimeNow() 00126 { 00127 } 00128 00129 00130 void 00131 VelocityGlobalFromRelative::getTime(long int *sec, long int *usec) 00132 { 00133 *sec = 0; 00134 *usec = 0; 00135 } 00136 00137 00138 void 00139 VelocityGlobalFromRelative::getVelocity(float *vel_x, float *vel_y) 00140 { 00141 if (vel_x != 0) { 00142 *vel_x = velocity_x; 00143 } 00144 if (vel_y != 0) { 00145 *vel_y = velocity_y; 00146 } 00147 } 00148 00149 00150 float 00151 VelocityGlobalFromRelative::getVelocityX() 00152 { 00153 return velocity_x; 00154 } 00155 00156 00157 float 00158 VelocityGlobalFromRelative::getVelocityY() 00159 { 00160 return velocity_y; 00161 } 00162 00163 00164 00165 void 00166 VelocityGlobalFromRelative::calc() 00167 { 00168 00169 relative_velocity->getVelocity( &rel_vel_x, &rel_vel_y ); 00170 sin_ori = sin( robot_ori ); 00171 cos_ori = cos( robot_ori ); 00172 rel_dist = relative_position->get_distance(); 00173 00174 velocity_x = rel_vel_x * cos_ori - rel_vel_y * sin_ori; 00175 velocity_y = rel_vel_x * sin_ori + rel_vel_y * cos_ori; 00176 00177 // applyKalmanFilter(); 00178 00179 } 00180 00181 00182 void 00183 VelocityGlobalFromRelative::reset() 00184 { 00185 // kalman_filter->reset(); 00186 avg_vx_sum = 0.f; 00187 avg_vx_num = 0; 00188 avg_vy_sum = 0.f; 00189 avg_vy_num = 0; 00190 velocity_x = 0.f; 00191 velocity_y = 0.f; 00192 } 00193 00194 00195 const char * 00196 VelocityGlobalFromRelative::getName() const 00197 { 00198 return "VelocityModel::VelocityGlobalFromRelative"; 00199 } 00200 00201 00202 coordsys_type_t 00203 VelocityGlobalFromRelative::getCoordinateSystem() 00204 { 00205 return COORDSYS_WORLD_CART; 00206 } 00207 00208 00209 /* 00210 void 00211 VelocityGlobalFromRelative::applyKalmanFilter() 00212 { 00213 avg_vx_sum += velocity_x; 00214 avg_vy_sum += velocity_y; 00215 00216 ++avg_vx_num; 00217 ++avg_vy_num; 00218 00219 avg_vx = avg_vx_sum / avg_vx_num; 00220 avg_vy = avg_vy_sum / avg_vy_num; 00221 00222 rx = (velocity_x - avg_vx) * robot_poseage; 00223 ry = (velocity_y - avg_vy) * robot_poseage; 00224 00225 kalman_filter->setProcessCovariance( rx * rx, ry * ry ); 00226 00227 rx = (velocity_x - avg_vx) * rel_dist; 00228 ry = (velocity_y - avg_vy) * rel_dist; 00229 00230 kalman_filter->setMeasurementCovariance( rx * rx, ry * ry ); 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 velocity_x = round( velocity_x * 10 ) / 10; 00240 velocity_y = round( velocity_y * 10 ) / 10; 00241 00242 if (isnan(velocity_x) || isinf(velocity_x) || 00243 isnan(velocity_y) || isinf(velocity_y) ) { 00244 reset(); 00245 } 00246 00247 } 00248 */ 00249 00250 } // end namespace firevision