Fawkes API Fawkes Development Version
globvelo.cpp
1
2/***************************************************************************
3 * globvelo.cpp - Implementation of velocity model based on global
4 * positions
5 *
6 * Created: Mon Sep 05 17:12:48 2005
7 * Copyright 2005 Tim Niemueller [www.niemueller.de]
8 *
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version. A runtime exception applies to
15 * this software (see LICENSE.GPL_WRE file mentioned below for details).
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Library General Public License for more details.
21 *
22 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23 */
24
25#include <fvmodels/velocity/globvelo.h>
26#include <utils/time/time.h>
27
28#include <cmath>
29
30namespace firevision {
31
32/** @class VelocityFromGlobal <fvmodels/velocity/globvelo.h>
33 * Velocity from global positions.
34 */
35
36/** Constructor.
37 * @param model global position model
38 * @param history_length maximum history length
39 * @param calc_interval calculation interval
40 */
42 unsigned int history_length,
43 unsigned int calc_interval)
44{
45 this->global_pos_model = model;
46 this->history_length = history_length;
47 this->calc_interval = calc_interval;
48
49 robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
50
51 velocity_x = velocity_y = 0.f;
52
53 /*
54 // initial variance for ball pos kf
55 kalman_filter = new kalmanFilter2Dim();
56 CMatrix<float> initialStateVarianceBall(2,2);
57 initialStateVarianceBall[0][0] = 2.00;
58 initialStateVarianceBall[1][0] = 0.00;
59 initialStateVarianceBall[0][1] = 0.00;
60 initialStateVarianceBall[1][1] = 2.00;
61 kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
62
63 // process noise for ball pos kf
64 CMatrix<float> processVarianceBall(2,2);
65 processVarianceBall[0][0] = 0.50;
66 processVarianceBall[1][0] = 0.00;
67 processVarianceBall[0][1] = 0.00;
68 processVarianceBall[1][1] = 0.50;
69 kalman_filter->setProcessCovariance( processVarianceBall );
70
71 kalman_filter->setMeasurementCovariance( 0.5, 0.5 );
72 */
73}
74
75/** Destructor. */
77{
78}
79
80void
81VelocityFromGlobal::setPanTilt(float pan, float tilt)
82{
83}
84
85void
86VelocityFromGlobal::setRobotPosition(float x, float y, float ori, timeval t)
87{
88 timeval _now;
89 gettimeofday(&_now, 0);
90 robot_pos_x = x;
91 robot_pos_y = y;
92 robot_pos_ori = ori;
93 robot_pos_age = fawkes::time_diff_sec(_now, t);
94}
95
96void
97VelocityFromGlobal::setRobotVelocity(float vel_x, float vel_y, timeval t)
98{
99}
100
101void
103{
104 now.tv_sec = t.tv_sec;
105 now.tv_usec = t.tv_usec;
106}
107
108void
110{
111 gettimeofday(&now, 0);
112}
113
114void
115VelocityFromGlobal::getTime(long int *sec, long int *usec)
116{
117 *sec = now.tv_sec;
118 *usec = now.tv_usec;
119}
120
121void
122VelocityFromGlobal::getVelocity(float *vel_x, float *vel_y)
123{
124 if (vel_x != 0) {
125 *vel_x = velocity_x;
126 }
127 if (vel_y != 0) {
128 *vel_y = velocity_y;
129 }
130}
131
132float
134{
135 return velocity_x;
136}
137
138float
140{
141 return velocity_y;
142}
143
144void
146{
147 // Gather needed data
148 current_x = global_pos_model->get_x();
149 current_y = global_pos_model->get_y();
150
151 last_x.push_back(current_x);
152 last_y.push_back(current_y);
153
154 last_time.push_back(now);
155
156 velocity_total_x = 0.f;
157 velocity_total_y = 0.f;
158 velocity_num = 0;
159
160 if (last_x.size() > calc_interval) {
161 // min of sizes
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];
166
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;
169 if (diff_usec < 0) {
170 diff_sec -= 1;
171 diff_usec += 1000000;
172 }
173
174 f_diff_sec = diff_sec + diff_usec / 1000000.f;
175
176 velocity_total_x += diff_x / f_diff_sec;
177 velocity_total_y += diff_y / f_diff_sec;
178 velocity_num++;
179 }
180 }
181
182 // Get average velocity
183 velocity_x = velocity_total_x / velocity_num;
184 velocity_y = velocity_total_y / velocity_num;
185
186 // applyKalmanFilter();
187
188 while (last_x.size() > history_length) {
189 last_x.erase(last_x.begin());
190 last_y.erase(last_y.begin());
191 }
192}
193
194void
196{
197 // kalman_filter->reset();
198}
199
200const char *
202{
203 return "VelocityModel::VelocityFromGlobal";
204}
205
206coordsys_type_t
208{
209 return COORDSYS_WORLD_CART;
210}
211
212/*
213void
214VelocityFromGlobal::applyKalmanFilter()
215{
216 kalman_filter->setMeasurementX( velocity_x );
217 kalman_filter->setMeasurementY( velocity_y );
218 kalman_filter->doCalculation();
219
220 velocity_x = kalman_filter->getStateX();
221 velocity_y = kalman_filter->getStateY();
222
223 if (isnan(velocity_x) || isinf(velocity_x)) {
224 kalman_filter->reset();
225 velocity_x = 0.f;
226 }
227
228 if (isnan(velocity_y) || isinf(velocity_y)) {
229 kalman_filter->reset();
230 velocity_y = 0.f;
231 }
232
233}
234*/
235
236} // end namespace firevision
Global Position Model Interface.
virtual float get_x() const =0
Get global x coordinate of object.
virtual float get_y() const =0
Get global y coordinate of object.
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
Definition: globvelo.cpp:115
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Definition: globvelo.cpp:145
virtual void setTimeNow()
Get current time from system.
Definition: globvelo.cpp:109
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Definition: globvelo.cpp:207
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
Definition: globvelo.cpp:122
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
Definition: globvelo.cpp:81
virtual const char * getName() const
Get name of velocity model.
Definition: globvelo.cpp:201
virtual float getVelocityX()
Get velocity of tracked object in X direction.
Definition: globvelo.cpp:133
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
Definition: globvelo.cpp:86
virtual void setTime(timeval t)
Set current time.
Definition: globvelo.cpp:102
virtual float getVelocityY()
Get velocity of tracked object in X direction.
Definition: globvelo.cpp:139
VelocityFromGlobal(GlobalPositionModel *model, unsigned int history_length, unsigned int calc_interval)
Constructor.
Definition: globvelo.cpp:41
virtual ~VelocityFromGlobal()
Destructor.
Definition: globvelo.cpp:76
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
Definition: globvelo.cpp:97
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
Definition: globvelo.cpp:195
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:41