00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
00034 #include <list>
00035
00036 namespace firevision {
00037 #if 0
00038 }
00039 #endif
00040
00041
00042 typedef struct {
00043 float x;
00044 float y;
00045 timeval t;
00046 } vel_postime_t;
00047
00048
00049 typedef struct {
00050 float vx;
00051 float vy;
00052 timeval t;
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
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
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142 };
00143
00144 }
00145
00146 #endif