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