25 #include <fvmodels/velocity/relvelo.h>
26 #include <utils/system/console_colors.h>
27 #include <utils/time/time.h>
35 namespace firevision {
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;
91 VelocityFromRelative::~VelocityFromRelative()
96 VelocityFromRelative::setPanTilt(
float pan,
float tilt)
101 VelocityFromRelative::setRobotPosition(
float x,
float y,
float ori, timeval t)
106 VelocityFromRelative::setRobotVelocity(
float rel_vel_x,
float rel_vel_y, timeval t)
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;
115 VelocityFromRelative::setTime(timeval t)
117 now.tv_sec = t.tv_sec;
118 now.tv_usec = t.tv_usec;
122 VelocityFromRelative::setTimeNow()
124 gettimeofday(&now, NULL);
128 VelocityFromRelative::getTime(
long int *sec,
long int *usec)
135 VelocityFromRelative::getVelocity(
float *vel_x,
float *vel_y)
146 VelocityFromRelative::getVelocityX()
152 VelocityFromRelative::getVelocityY()
158 VelocityFromRelative::calc()
193 cur_ball_x = relative_pos_model->get_x();
194 cur_ball_y = relative_pos_model->get_y();
195 cur_ball_dist = relative_pos_model->get_distance();
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());
357 VelocityFromRelative::reset()
370 ball_history.clear();
374 VelocityFromRelative::getName()
const
376 return "VelocityModel::VelocityFromRelative";
380 VelocityFromRelative::getCoordinateSystem()
382 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.