25 #include <fvmodels/velocity/relvelo.h> 27 #include <utils/system/console_colors.h> 28 #include <utils/time/time.h> 50 unsigned int max_history_length,
51 unsigned int calc_interval)
53 this->relative_pos_model = model;
54 this->max_history_length = max_history_length;
55 this->calc_interval = calc_interval;
59 robot_rel_vel_x = robot_rel_vel_y = 0.f;
61 velocity_x = velocity_y = 0.f;
96 VelocityFromRelative::~VelocityFromRelative()
102 VelocityFromRelative::setPanTilt(
float pan,
float tilt)
108 VelocityFromRelative::setRobotPosition(
float x,
float y,
float ori, timeval t)
114 VelocityFromRelative::setRobotVelocity(
float rel_vel_x,
float rel_vel_y, timeval t)
116 robot_rel_vel_x = rel_vel_x;
117 robot_rel_vel_y = rel_vel_y;
118 robot_rel_vel_t.tv_sec = t.tv_sec;
119 robot_rel_vel_t.tv_usec = t.tv_usec;
123 VelocityFromRelative::setTime(timeval t)
125 now.tv_sec = t.tv_sec;
126 now.tv_usec = t.tv_usec;
131 VelocityFromRelative::setTimeNow()
133 gettimeofday(&now, NULL);
138 VelocityFromRelative::getTime(
long int *sec,
long int *usec)
146 VelocityFromRelative::getVelocity(
float *vel_x,
float *vel_y)
158 VelocityFromRelative::getVelocityX()
165 VelocityFromRelative::getVelocityY()
172 VelocityFromRelative::calc()
207 cur_ball_x = relative_pos_model->get_x();
208 cur_ball_y = relative_pos_model->get_y();
209 cur_ball_dist = relative_pos_model->get_distance();
211 if ( isnan(cur_ball_x) || isinf(cur_ball_x) ||
212 isnan(cur_ball_y) || isinf(cur_ball_y) ||
213 isnan(cur_ball_dist) || isinf(cur_ball_dist) ) {
221 if (last_available) {
223 proj_x = last_x + velocity_x * proj_time_diff_sec;
224 proj_y = last_y + velocity_y * proj_time_diff_sec;
225 last_proj_error_x = cur_ball_x - proj_x;
226 last_proj_error_y = cur_ball_y - proj_y;
227 last_available =
false;
229 last_proj_error_x = cur_ball_x;
230 last_proj_error_y = cur_ball_x;
238 vpt->
t.tv_sec = now.tv_sec;
239 vpt->
t.tv_usec = now.tv_usec;
240 ball_history.push_front( vpt );
243 if (ball_history.size() >= 2) {
253 vel_last_time.tv_sec = robot_rel_vel_t.tv_sec;
254 vel_last_time.tv_usec = robot_rel_vel_t.tv_usec;
260 unsigned int step = 0;
261 for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) {
265 if ( (time_diff > 0) && (time_diff < f_diff_sec) ) {
266 f_diff_sec = time_diff;
270 if (step != calc_interval) {
281 if ((young != NULL) && (old != NULL)) {
284 diff_x = young->
x - old->
x;
285 diff_y = young->
y - old->
y;
289 velocity_x = diff_x / f_diff_sec;
290 velocity_y = diff_y / f_diff_sec;
294 velocity_x += robot_rel_vel_x;
295 velocity_y += robot_rel_vel_y;
297 velocity_x -= last_proj_error_x * proj_time_diff_sec;
298 velocity_y -= last_proj_error_y * proj_time_diff_sec;
331 last_time.tv_sec = now.tv_sec;
332 last_time.tv_usec = now.tv_usec;
333 last_available =
true;
342 if (bh_it != ball_history.end()) {
343 ball_history.erase(bh_it, ball_history.end());
364 if (ball_history.size() > max_history_length) {
365 bh_it = ball_history.begin();
366 for (
unsigned int i = 0; i < max_history_length; ++i) {
369 ball_history.erase(bh_it, ball_history.end());
376 VelocityFromRelative::reset()
389 ball_history.clear();
394 VelocityFromRelative::getName()
const 396 return "VelocityModel::VelocityFromRelative";
401 VelocityFromRelative::getCoordinateSystem()
403 return COORDSYS_ROBOT_CART;
Relative Position Model Interface.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.