Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * relvelo.h - A simple velocity model using the relative coordinates and 00004 * robot velocity 00005 * 00006 * Created: Tue Oct 04 15:49:23 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 #ifndef __FIREVISION_MODELS_VELOCITY_RELATIVE_H_ 00026 #define __FIREVISION_MODELS_VELOCITY_RELATIVE_H_ 00027 00028 #include <models/velocity/velocitymodel.h> 00029 #include <models/relative_position/relativepositionmodel.h> 00030 00031 #include <fvutils/base/types.h> 00032 00033 // include <utils/kalman_filter/ckalman_filter_2dim.h> 00034 #include <list> 00035 00036 namespace firevision { 00037 #if 0 /* just to make Emacs auto-indent happy */ 00038 } 00039 #endif 00040 00041 /** Position/time tuple. */ 00042 typedef struct { 00043 float x; /**< x pos */ 00044 float y; /**< y pos */ 00045 timeval t; /**< time */ 00046 } vel_postime_t; 00047 00048 /** Velocity/time tuple. */ 00049 typedef struct { 00050 float vx; /**< vx in m/s */ 00051 float vy; /**< vy in m/s */ 00052 timeval t; /**< time */ 00053 } vel_veltime_t; 00054 00055 class VelocityFromRelative : public VelocityModel 00056 { 00057 public: 00058 VelocityFromRelative(RelativePositionModel* model, unsigned int max_history_length, unsigned int calc_interval); 00059 virtual ~VelocityFromRelative(); 00060 00061 virtual const char * getName() const; 00062 00063 virtual void setRobotPosition(float x, float y, float ori, timeval t); 00064 virtual void setRobotVelocity(float vel_x, float vel_y, timeval t); 00065 virtual void setPanTilt(float pan, float tilt); 00066 virtual void setTime(timeval t); 00067 virtual void setTimeNow(); 00068 virtual void getTime(long int *sec, long int *usec); 00069 00070 virtual void getVelocity(float *vel_x, float *vel_y); 00071 00072 virtual float getVelocityX(); 00073 virtual float getVelocityY(); 00074 00075 virtual void calc(); 00076 virtual void reset(); 00077 00078 virtual coordsys_type_t getCoordinateSystem(); 00079 00080 private: 00081 RelativePositionModel *relative_pos_model; 00082 00083 float robot_rel_vel_x; 00084 float robot_rel_vel_y; 00085 timeval robot_rel_vel_t; 00086 timeval vel_last_time; 00087 00088 timeval now; 00089 std::list<vel_postime_t *> ball_history; 00090 std::list<vel_postime_t *>::iterator bh_it; 00091 00092 int diff_sec; 00093 int diff_usec; 00094 00095 float f_diff_sec; 00096 00097 unsigned int max_history_length; 00098 unsigned int calc_interval; 00099 00100 float cur_ball_x; 00101 float cur_ball_y; 00102 float cur_ball_dist; 00103 00104 // for projection 00105 bool last_available; 00106 timeval last_time; 00107 float last_x; 00108 float last_y; 00109 float proj_x; 00110 float proj_y; 00111 float last_proj_error_x; 00112 float last_proj_error_y; 00113 float proj_time_diff_sec; 00114 00115 float diff_x; 00116 float diff_y; 00117 00118 float velocity_x; 00119 float velocity_y; 00120 00121 float avg_vx_sum; 00122 float avg_vy_sum; 00123 float avg_vx; 00124 float avg_vy; 00125 unsigned int avg_vx_num; 00126 unsigned int avg_vy_num; 00127 float rx; 00128 float ry; 00129 float age_factor; 00130 00131 /* 00132 bool kalman_enabled; 00133 float var_proc_x; 00134 float var_proc_y; 00135 float var_meas_x; 00136 float var_meas_y; 00137 kalmanFilter2Dim *kalman_filter; 00138 00139 void applyKalmanFilter(); 00140 */ 00141 00142 }; 00143 00144 } // end namespace firevision 00145 00146 #endif