25#include <fvmodels/velocity/relvelo.h>
26#include <utils/system/console_colors.h>
27#include <utils/time/time.h>
47 unsigned int max_history_length,
48 unsigned int calc_interval)
50 this->relative_pos_model = model;
51 this->max_history_length = max_history_length;
52 this->calc_interval = calc_interval;
56 robot_rel_vel_x = robot_rel_vel_y = 0.f;
58 velocity_x = velocity_y = 0.f;
108 robot_rel_vel_x = rel_vel_x;
109 robot_rel_vel_y = rel_vel_y;
110 robot_rel_vel_t.tv_sec = t.tv_sec;
111 robot_rel_vel_t.tv_usec = t.tv_usec;
117 now.tv_sec = t.tv_sec;
118 now.tv_usec = t.tv_usec;
124 gettimeofday(&now, NULL);
193 cur_ball_x = relative_pos_model->
get_x();
194 cur_ball_y = relative_pos_model->
get_y();
197 if (isnan(cur_ball_x) || isinf(cur_ball_x) || isnan(cur_ball_y) || isinf(cur_ball_y)
198 || isnan(cur_ball_dist) || isinf(cur_ball_dist)) {
206 if (last_available) {
208 proj_x = last_x + velocity_x * proj_time_diff_sec;
209 proj_y = last_y + velocity_y * proj_time_diff_sec;
210 last_proj_error_x = cur_ball_x - proj_x;
211 last_proj_error_y = cur_ball_y - proj_y;
212 last_available =
false;
214 last_proj_error_x = cur_ball_x;
215 last_proj_error_y = cur_ball_x;
223 vpt->
t.tv_sec = now.tv_sec;
224 vpt->
t.tv_usec = now.tv_usec;
225 ball_history.push_front(vpt);
227 if (ball_history.size() >= 2) {
236 vel_last_time.tv_sec = robot_rel_vel_t.tv_sec;
237 vel_last_time.tv_usec = robot_rel_vel_t.tv_usec;
239 f_diff_sec = numeric_limits<float>::max();
243 unsigned int step = 0;
244 for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) {
248 if ((time_diff > 0) && (time_diff < f_diff_sec)) {
249 f_diff_sec = time_diff;
253 if (step != calc_interval) {
264 if ((young != NULL) && (old != NULL)) {
267 diff_x = young->
x - old->
x;
268 diff_y = young->
y - old->
y;
272 velocity_x = diff_x / f_diff_sec;
273 velocity_y = diff_y / f_diff_sec;
277 velocity_x += robot_rel_vel_x;
278 velocity_y += robot_rel_vel_y;
280 velocity_x -= last_proj_error_x * proj_time_diff_sec;
281 velocity_y -= last_proj_error_y * proj_time_diff_sec;
314 last_time.tv_sec = now.tv_sec;
315 last_time.tv_usec = now.tv_usec;
316 last_available =
true;
325 if (bh_it != ball_history.end()) {
326 ball_history.erase(bh_it, ball_history.end());
347 if (ball_history.size() > max_history_length) {
348 bh_it = ball_history.begin();
349 for (
unsigned int i = 0; i < max_history_length; ++i) {
352 ball_history.erase(bh_it, ball_history.end());
370 ball_history.clear();
376 return "VelocityModel::VelocityFromRelative";
382 return COORDSYS_ROBOT_CART;
Relative Position Model Interface.
virtual float get_y() const =0
Get relative Y coordinate of object.
virtual float get_x() const =0
Get relative X coordinate of object.
virtual float get_distance() const =0
Get distance to object.
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
virtual void setTime(timeval t)
Set current time.
virtual float getVelocityX()
Get velocity of tracked object in X direction.
virtual ~VelocityFromRelative()
Destructor.
VelocityFromRelative(RelativePositionModel *model, unsigned int max_history_length, unsigned int calc_interval)
Constructor.
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
virtual float getVelocityY()
Get velocity of tracked object in X direction.
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
virtual const char * getName() const
Get name of velocity model.
virtual void setTimeNow()
Get current time from system.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.