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