25 #include <fvmodels/velocity/globvelo.h>
26 #include <utils/time/time.h>
30 namespace firevision {
42 unsigned int history_length,
43 unsigned int calc_interval)
45 this->global_pos_model = model;
46 this->history_length = history_length;
47 this->calc_interval = calc_interval;
49 robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
51 velocity_x = velocity_y = 0.f;
89 gettimeofday(&_now, 0);
104 now.tv_sec = t.tv_sec;
105 now.tv_usec = t.tv_usec;
111 gettimeofday(&now, 0);
148 current_x = global_pos_model->
get_x();
149 current_y = global_pos_model->
get_y();
151 last_x.push_back(current_x);
152 last_y.push_back(current_y);
154 last_time.push_back(now);
156 velocity_total_x = 0.f;
157 velocity_total_y = 0.f;
160 if (last_x.size() > calc_interval) {
162 unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size();
163 for (
unsigned int i = calc_interval; i < m; i += calc_interval) {
164 diff_x = last_x[i] - last_x[i - calc_interval];
165 diff_y = last_y[i] - last_y[i - calc_interval];
167 diff_sec = last_time[i].tv_sec - last_time[i - calc_interval].tv_sec;
168 diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
171 diff_usec += 1000000;
174 f_diff_sec = diff_sec + diff_usec / 1000000.f;
176 velocity_total_x += diff_x / f_diff_sec;
177 velocity_total_y += diff_y / f_diff_sec;
183 velocity_x = velocity_total_x / velocity_num;
184 velocity_y = velocity_total_y / velocity_num;
188 while (last_x.size() > history_length) {
189 last_x.erase(last_x.begin());
190 last_y.erase(last_y.begin());
203 return "VelocityModel::VelocityFromGlobal";
209 return COORDSYS_WORLD_CART;