globfromrel.cpp

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

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1