25 #include <fvmodels/velocity/relvelo.h> 27 #include <utils/system/console_colors.h> 28 #include <utils/time/time.h> 52 unsigned int max_history_length,
53 unsigned int calc_interval)
55 this->relative_pos_model = model;
56 this->max_history_length = max_history_length;
57 this->calc_interval = calc_interval;
61 robot_rel_vel_x = robot_rel_vel_y = 0.f;
63 velocity_x = velocity_y = 0.f;
98 VelocityFromRelative::~VelocityFromRelative()
104 VelocityFromRelative::setPanTilt(
float pan,
float tilt)
110 VelocityFromRelative::setRobotPosition(
float x,
float y,
float ori, timeval t)
116 VelocityFromRelative::setRobotVelocity(
float rel_vel_x,
float rel_vel_y, timeval t)
118 robot_rel_vel_x = rel_vel_x;
119 robot_rel_vel_y = rel_vel_y;
120 robot_rel_vel_t.tv_sec = t.tv_sec;
121 robot_rel_vel_t.tv_usec = t.tv_usec;
125 VelocityFromRelative::setTime(timeval t)
127 now.tv_sec = t.tv_sec;
128 now.tv_usec = t.tv_usec;
133 VelocityFromRelative::setTimeNow()
135 gettimeofday(&now, NULL);
140 VelocityFromRelative::getTime(
long int *sec,
long int *usec)
148 VelocityFromRelative::getVelocity(
float *vel_x,
float *vel_y)
160 VelocityFromRelative::getVelocityX()
167 VelocityFromRelative::getVelocityY()
174 VelocityFromRelative::calc()
209 cur_ball_x = relative_pos_model->get_x();
210 cur_ball_y = relative_pos_model->get_y();
211 cur_ball_dist = relative_pos_model->get_distance();
213 if ( isnan(cur_ball_x) || isinf(cur_ball_x) ||
214 isnan(cur_ball_y) || isinf(cur_ball_y) ||
215 isnan(cur_ball_dist) || isinf(cur_ball_dist) ) {
223 if (last_available) {
225 proj_x = last_x + velocity_x * proj_time_diff_sec;
226 proj_y = last_y + velocity_y * proj_time_diff_sec;
227 last_proj_error_x = cur_ball_x - proj_x;
228 last_proj_error_y = cur_ball_y - proj_y;
229 last_available =
false;
231 last_proj_error_x = cur_ball_x;
232 last_proj_error_y = cur_ball_x;
240 vpt->
t.tv_sec = now.tv_sec;
241 vpt->
t.tv_usec = now.tv_usec;
242 ball_history.push_front( vpt );
245 if (ball_history.size() >= 2) {
255 vel_last_time.tv_sec = robot_rel_vel_t.tv_sec;
256 vel_last_time.tv_usec = robot_rel_vel_t.tv_usec;
258 f_diff_sec = numeric_limits<float>::max();
262 unsigned int step = 0;
263 for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) {
267 if ( (time_diff > 0) && (time_diff < f_diff_sec) ) {
268 f_diff_sec = time_diff;
272 if (step != calc_interval) {
283 if ((young != NULL) && (old != NULL)) {
286 diff_x = young->
x - old->
x;
287 diff_y = young->
y - old->
y;
291 velocity_x = diff_x / f_diff_sec;
292 velocity_y = diff_y / f_diff_sec;
296 velocity_x += robot_rel_vel_x;
297 velocity_y += robot_rel_vel_y;
299 velocity_x -= last_proj_error_x * proj_time_diff_sec;
300 velocity_y -= last_proj_error_y * proj_time_diff_sec;
333 last_time.tv_sec = now.tv_sec;
334 last_time.tv_usec = now.tv_usec;
335 last_available =
true;
344 if (bh_it != ball_history.end()) {
345 ball_history.erase(bh_it, ball_history.end());
366 if (ball_history.size() > max_history_length) {
367 bh_it = ball_history.begin();
368 for (
unsigned int i = 0; i < max_history_length; ++i) {
371 ball_history.erase(bh_it, ball_history.end());
378 VelocityFromRelative::reset()
391 ball_history.clear();
396 VelocityFromRelative::getName()
const 398 return "VelocityModel::VelocityFromRelative";
403 VelocityFromRelative::getCoordinateSystem()
405 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.