26 #include <fvmodels/velocity/globvelo.h> 28 #include <utils/time/time.h> 45 unsigned int history_length,
46 unsigned int calc_interval)
48 this->global_pos_model = model;
49 this->history_length = history_length;
50 this->calc_interval = calc_interval;
52 robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
54 velocity_x = velocity_y = 0.f;
80 VelocityFromGlobal::~VelocityFromGlobal()
86 VelocityFromGlobal::setPanTilt(
float pan,
float tilt)
92 VelocityFromGlobal::setRobotPosition(
float x,
float y,
float ori, timeval t)
95 gettimeofday(&_now, 0);
104 VelocityFromGlobal::setRobotVelocity(
float vel_x,
float vel_y, timeval t)
110 VelocityFromGlobal::setTime(timeval t)
112 now.tv_sec = t.tv_sec;
113 now.tv_usec = t.tv_usec;
118 VelocityFromGlobal::setTimeNow()
120 gettimeofday(&now, 0);
125 VelocityFromGlobal::getTime(
long int *sec,
long int *usec)
133 VelocityFromGlobal::getVelocity(
float *vel_x,
float *vel_y)
145 VelocityFromGlobal::getVelocityX()
152 VelocityFromGlobal::getVelocityY()
160 VelocityFromGlobal::calc()
164 current_x = global_pos_model->get_x();
165 current_y = global_pos_model->get_y();
167 last_x.push_back( current_x );
168 last_y.push_back( current_y );
170 last_time.push_back(now);
172 velocity_total_x = 0.f;
173 velocity_total_y = 0.f;
176 if (last_x.size() > calc_interval) {
178 unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size() ;
179 for (
unsigned int i = calc_interval; i < m; i += calc_interval) {
180 diff_x = last_x[i] - last_x[i - calc_interval];
181 diff_y = last_y[i] - last_y[i - calc_interval];
183 diff_sec = last_time[i].tv_sec - last_time[i - calc_interval].tv_sec;
184 diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
187 diff_usec += 1000000;
190 f_diff_sec = diff_sec + diff_usec / 1000000.f;
192 velocity_total_x += diff_x / f_diff_sec;
193 velocity_total_y += diff_y / f_diff_sec;
199 velocity_x = velocity_total_x / velocity_num;
200 velocity_y = velocity_total_y / velocity_num;
204 while (last_x.size() > history_length) {
205 last_x.erase( last_x.begin() );
206 last_y.erase( last_y.begin() );
213 VelocityFromGlobal::reset()
220 VelocityFromGlobal::getName()
const 222 return "VelocityModel::VelocityFromGlobal";
227 VelocityFromGlobal::getCoordinateSystem()
229 return COORDSYS_WORLD_CART;
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Global Position Model Interface.