Fawkes API
Fawkes Development Version
|
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 getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
virtual float get_x() const =0
Get global x coordinate of object.
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
VelocityFromGlobal(GlobalPositionModel *model, unsigned int history_length, unsigned int calc_interval)
Constructor.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
virtual const char * getName() const
Get name of velocity model.
virtual void setTimeNow()
Get current time from system.
Global Position Model Interface.
virtual void setTime(timeval t)
Set current time.
virtual float get_y() const =0
Get global y coordinate of object.
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
virtual float getVelocityX()
Get velocity of tracked object in X direction.
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
virtual ~VelocityFromGlobal()
Destructor.
virtual float getVelocityY()
Get velocity of tracked object in X direction.