Fawkes API  Fawkes Development Version
globfromrel.cpp
1 
2 /***************************************************************************
3  * globfromrel.cpp - Implementation of velocity model based on relative
4  * ball positions and relative robot velocity
5  *
6  * Created: Fri Oct 21 11:19:03 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 <core/exception.h>
26 #include <cmath>
27 #include <fvmodels/velocity/globfromrel.h>
28 #include <utils/time/time.h>
29 
30 // #include "utils/system/console_colors.h"
31 // #include "utils/system/time.h"
32 
33 using namespace std;
34 using namespace fawkes;
35 
36 namespace firevision {
37 #if 0 /* just to make Emacs auto-indent happy */
38 }
39 #endif
40 
41 /** @class VelocityGlobalFromRelative <fvmodels/velocity/globfromrel.h>
42  * Global velocity from relative velocities.
43  */
44 
45 /** Destructor.
46  * @param rel_velo_model relative velocity model
47  * @param rel_pos_model relative position model
48  */
49 VelocityGlobalFromRelative::VelocityGlobalFromRelative(VelocityModel *rel_velo_model,
50  RelativePositionModel *rel_pos_model)
51 {
52  this->relative_velocity = rel_velo_model;
53  this->relative_position = rel_pos_model;
54 
55  if ( rel_velo_model->getCoordinateSystem() != COORDSYS_ROBOT_CART ) {
56  /*
57  cout << cblue << "VelocityGlobalFromRelative::Constructor: " << cred
58  << "Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!"
59  << cnormal << endl;
60  */
61  throw Exception("Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!");
62  }
63 
64  robot_ori = robot_poseage = 0.f;
65 
66  velocity_x = velocity_y = 0.f;
67 
68  /*
69  // initial variance for ball pos kf
70  kalman_filter = new kalmanFilter2Dim();
71  CMatrix<float> initialStateVarianceBall(2,2);
72  initialStateVarianceBall[0][0] = 5.00;
73  initialStateVarianceBall[1][0] = 0.00;
74  initialStateVarianceBall[0][1] = 0.00;
75  initialStateVarianceBall[1][1] = 5.00;
76  kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
77 
78  // process noise for ball pos kf, initial estimates, refined in calc()
79  kalman_filter->setProcessCovariance( 1.f, 1.f );
80  kalman_filter->setMeasurementCovariance( 4.f, 4.f );
81 
82  avg_vx_sum = 0.f;
83  avg_vx_num = 0;
84 
85  avg_vy_sum = 0.f;
86  avg_vy_num = 0;
87  */
88 }
89 
90 
91 /** Destructor. */
92 VelocityGlobalFromRelative::~VelocityGlobalFromRelative()
93 {
94 }
95 
96 
97 void
98 VelocityGlobalFromRelative::setPanTilt(float pan, float tilt)
99 {
100 }
101 
102 
103 void
104 VelocityGlobalFromRelative::setRobotPosition(float x, float y, float ori, timeval t)
105 {
106  timeval now;
107  gettimeofday(&now, 0);
108  robot_ori = ori;
109  robot_poseage = time_diff_sec(now, t);
110 }
111 
112 
113 void
114 VelocityGlobalFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t)
115 {
116 }
117 
118 void
119 VelocityGlobalFromRelative::setTime(timeval t)
120 {
121 }
122 
123 
124 void
125 VelocityGlobalFromRelative::setTimeNow()
126 {
127 }
128 
129 
130 void
131 VelocityGlobalFromRelative::getTime(long int *sec, long int *usec)
132 {
133  *sec = 0;
134  *usec = 0;
135 }
136 
137 
138 void
139 VelocityGlobalFromRelative::getVelocity(float *vel_x, float *vel_y)
140 {
141  if (vel_x != 0) {
142  *vel_x = velocity_x;
143  }
144  if (vel_y != 0) {
145  *vel_y = velocity_y;
146  }
147 }
148 
149 
150 float
151 VelocityGlobalFromRelative::getVelocityX()
152 {
153  return velocity_x;
154 }
155 
156 
157 float
158 VelocityGlobalFromRelative::getVelocityY()
159 {
160  return velocity_y;
161 }
162 
163 
164 
165 void
166 VelocityGlobalFromRelative::calc()
167 {
168 
169  relative_velocity->getVelocity( &rel_vel_x, &rel_vel_y );
170  sin_ori = sin( robot_ori );
171  cos_ori = cos( robot_ori );
172  rel_dist = relative_position->get_distance();
173 
174  velocity_x = rel_vel_x * cos_ori - rel_vel_y * sin_ori;
175  velocity_y = rel_vel_x * sin_ori + rel_vel_y * cos_ori;
176 
177  // applyKalmanFilter();
178 
179 }
180 
181 
182 void
183 VelocityGlobalFromRelative::reset()
184 {
185  // kalman_filter->reset();
186  avg_vx_sum = 0.f;
187  avg_vx_num = 0;
188  avg_vy_sum = 0.f;
189  avg_vy_num = 0;
190  velocity_x = 0.f;
191  velocity_y = 0.f;
192 }
193 
194 
195 const char *
196 VelocityGlobalFromRelative::getName() const
197 {
198  return "VelocityModel::VelocityGlobalFromRelative";
199 }
200 
201 
202 coordsys_type_t
203 VelocityGlobalFromRelative::getCoordinateSystem()
204 {
205  return COORDSYS_WORLD_CART;
206 }
207 
208 
209 /*
210 void
211 VelocityGlobalFromRelative::applyKalmanFilter()
212 {
213  avg_vx_sum += velocity_x;
214  avg_vy_sum += velocity_y;
215 
216  ++avg_vx_num;
217  ++avg_vy_num;
218 
219  avg_vx = avg_vx_sum / avg_vx_num;
220  avg_vy = avg_vy_sum / avg_vy_num;
221 
222  rx = (velocity_x - avg_vx) * robot_poseage;
223  ry = (velocity_y - avg_vy) * robot_poseage;
224 
225  kalman_filter->setProcessCovariance( rx * rx, ry * ry );
226 
227  rx = (velocity_x - avg_vx) * rel_dist;
228  ry = (velocity_y - avg_vy) * rel_dist;
229 
230  kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
231 
232  kalman_filter->setMeasurementX( velocity_x );
233  kalman_filter->setMeasurementY( velocity_y );
234  kalman_filter->doCalculation();
235 
236  velocity_x = kalman_filter->getStateX();
237  velocity_y = kalman_filter->getStateY();
238 
239  velocity_x = round( velocity_x * 10 ) / 10;
240  velocity_y = round( velocity_y * 10 ) / 10;
241 
242  if (isnan(velocity_x) || isinf(velocity_x) ||
243  isnan(velocity_y) || isinf(velocity_y) ) {
244  reset();
245  }
246 
247 }
248 */
249 
250 } // end namespace firevision
virtual coordsys_type_t getCoordinateSystem()=0
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Fawkes library namespace.
STL namespace.
Velocity model interface.
Definition: velocitymodel.h:35
Relative Position Model Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:40