25 #include <fvmodels/velocity/globvelo.h> 26 #include <utils/time/time.h> 30 namespace firevision {
42 unsigned int history_length,
43 unsigned int calc_interval)
45 this->global_pos_model = model;
46 this->history_length = history_length;
47 this->calc_interval = calc_interval;
49 robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
51 velocity_x = velocity_y = 0.f;
89 gettimeofday(&_now, 0);
104 now.tv_sec = t.tv_sec;
105 now.tv_usec = t.tv_usec;
111 gettimeofday(&now, 0);
148 current_x = global_pos_model->
get_x();
149 current_y = global_pos_model->
get_y();
151 last_x.push_back(current_x);
152 last_y.push_back(current_y);
154 last_time.push_back(now);
156 velocity_total_x = 0.f;
157 velocity_total_y = 0.f;
160 if (last_x.size() > calc_interval) {
162 unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size();
163 for (
unsigned int i = calc_interval; i < m; i += calc_interval) {
164 diff_x = last_x[i] - last_x[i - calc_interval];
165 diff_y = last_y[i] - last_y[i - calc_interval];
167 diff_sec = last_time[i].tv_sec - last_time[i - calc_interval].tv_sec;
168 diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
171 diff_usec += 1000000;
174 f_diff_sec = diff_sec + diff_usec / 1000000.f;
176 velocity_total_x += diff_x / f_diff_sec;
177 velocity_total_y += diff_y / f_diff_sec;
183 velocity_x = velocity_total_x / velocity_num;
184 velocity_y = velocity_total_y / velocity_num;
188 while (last_x.size() > history_length) {
189 last_x.erase(last_x.begin());
190 last_y.erase(last_y.begin());
203 return "VelocityModel::VelocityFromGlobal";
209 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.