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 
30 namespace 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  */
41 VelocityFromGlobal::VelocityFromGlobal(GlobalPositionModel *model,
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 
80 void
81 VelocityFromGlobal::setPanTilt(float pan, float tilt)
82 {
83 }
84 
85 void
86 VelocityFromGlobal::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 
96 void
97 VelocityFromGlobal::setRobotVelocity(float vel_x, float vel_y, timeval t)
98 {
99 }
100 
101 void
103 {
104  now.tv_sec = t.tv_sec;
105  now.tv_usec = t.tv_usec;
106 }
107 
108 void
110 {
111  gettimeofday(&now, 0);
112 }
113 
114 void
115 VelocityFromGlobal::getTime(long int *sec, long int *usec)
116 {
117  *sec = now.tv_sec;
118  *usec = now.tv_usec;
119 }
120 
121 void
122 VelocityFromGlobal::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 
132 float
134 {
135  return velocity_x;
136 }
137 
138 float
140 {
141  return velocity_y;
142 }
143 
144 void
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 
194 void
196 {
197  // kalman_filter->reset();
198 }
199 
200 const char *
202 {
203  return "VelocityModel::VelocityFromGlobal";
204 }
205 
206 coordsys_type_t
208 {
209  return COORDSYS_WORLD_CART;
210 }
211 
212 /*
213 void
214 VelocityFromGlobal::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
firevision::VelocityFromGlobal::getVelocity
virtual void getVelocity(float *vel_x, float *vel_y)
Definition: globvelo.cpp:127
firevision::GlobalPositionModel::get_x
virtual float get_x() const =0
firevision::VelocityFromGlobal::getTime
virtual void getTime(long int *sec, long int *usec)
Definition: globvelo.cpp:120
firevision::VelocityFromGlobal::getCoordinateSystem
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Definition: globvelo.cpp:212
firevision::VelocityFromGlobal::VelocityFromGlobal
VelocityFromGlobal(GlobalPositionModel *model, unsigned int history_length, unsigned int calc_interval)
Constructor.
Definition: globvelo.cpp:46
fawkes::time_diff_sec
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:45
firevision::VelocityFromGlobal::getName
virtual const char * getName() const
Definition: globvelo.cpp:206
firevision::VelocityFromGlobal::setTimeNow
virtual void setTimeNow()
Definition: globvelo.cpp:114
firevision::VelocityFromGlobal::setTime
virtual void setTime(timeval t)
Definition: globvelo.cpp:107
firevision::GlobalPositionModel::get_y
virtual float get_y() const =0
firevision::VelocityFromGlobal::setRobotVelocity
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Definition: globvelo.cpp:102
firevision::VelocityFromGlobal::calc
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Definition: globvelo.cpp:150
firevision::VelocityFromGlobal::reset
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
Definition: globvelo.cpp:200
firevision::VelocityFromGlobal::getVelocityX
virtual float getVelocityX()
Definition: globvelo.cpp:138
firevision::VelocityFromGlobal::setPanTilt
virtual void setPanTilt(float pan, float tilt)
Definition: globvelo.cpp:86
firevision::VelocityFromGlobal::setRobotPosition
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Definition: globvelo.cpp:91
firevision::VelocityFromGlobal::~VelocityFromGlobal
virtual ~VelocityFromGlobal()
Destructor.
Definition: globvelo.cpp:81
firevision::VelocityFromGlobal::getVelocityY
virtual float getVelocityY()
Definition: globvelo.cpp:144