26 #include <fvmodels/velocity/globvelo.h> 28 #include <utils/time/time.h> 45 unsigned int history_length,
46 unsigned int calc_interval)
48 this->global_pos_model = model;
49 this->history_length = history_length;
50 this->calc_interval = calc_interval;
52 robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
54 velocity_x = velocity_y = 0.f;
95 gettimeofday(&_now, 0);
112 now.tv_sec = t.tv_sec;
113 now.tv_usec = t.tv_usec;
120 gettimeofday(&now, 0);
164 current_x = global_pos_model->
get_x();
165 current_y = global_pos_model->
get_y();
167 last_x.push_back( current_x );
168 last_y.push_back( current_y );
170 last_time.push_back(now);
172 velocity_total_x = 0.f;
173 velocity_total_y = 0.f;
176 if (last_x.size() > calc_interval) {
178 unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size() ;
179 for (
unsigned int i = calc_interval; i < m; i += calc_interval) {
180 diff_x = last_x[i] - last_x[i - calc_interval];
181 diff_y = last_y[i] - last_y[i - calc_interval];
183 diff_sec = last_time[i].tv_sec - last_time[i - calc_interval].tv_sec;
184 diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
187 diff_usec += 1000000;
190 f_diff_sec = diff_sec + diff_usec / 1000000.f;
192 velocity_total_x += diff_x / f_diff_sec;
193 velocity_total_y += diff_y / f_diff_sec;
199 velocity_x = velocity_total_x / velocity_num;
200 velocity_y = velocity_total_y / velocity_num;
204 while (last_x.size() > history_length) {
205 last_x.erase( last_x.begin() );
206 last_y.erase( last_y.begin() );
222 return "VelocityModel::VelocityFromGlobal";
229 return COORDSYS_WORLD_CART;
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
virtual ~VelocityFromGlobal()
Destructor.
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
virtual const char * getName() const
Get name of velocity model.
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
virtual float getVelocityY()
Get velocity of tracked object in X direction.
virtual void setTime(timeval t)
Set current time.
virtual void setTimeNow()
Get current time from system.
virtual float get_x() const =0
Get global x coordinate of object.
virtual float get_y() const =0
Get global y coordinate of object.
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Global Position Model Interface.
VelocityFromGlobal(GlobalPositionModel *model, unsigned int history_length, unsigned int calc_interval)
Constructor.
virtual float getVelocityX()
Get velocity of tracked object in X direction.
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.