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
34using namespace std;
35using namespace fawkes;
36
37namespace 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 */
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. */
91{
92}
93
94void
96{
97}
98
99void
100VelocityGlobalFromRelative::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
108void
109VelocityGlobalFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t)
110{
111}
112
113void
115{
116}
117
118void
120{
121}
122
123void
124VelocityGlobalFromRelative::getTime(long int *sec, long int *usec)
125{
126 *sec = 0;
127 *usec = 0;
128}
129
130void
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
141float
143{
144 return velocity_x;
145}
146
147float
149{
150 return velocity_y;
151}
152
153void
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
167void
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
179const char *
181{
182 return "VelocityModel::VelocityGlobalFromRelative";
183}
184
185coordsys_type_t
187{
188 return COORDSYS_WORLD_CART;
189}
190
191/*
192void
193VelocityGlobalFromRelative::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
Base class for exceptions in Fawkes.
Definition: exception.h:36
Relative Position Model Interface.
virtual float get_distance() const =0
Get distance to object.
virtual void setTime(timeval t)
Set current time.
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
Definition: globfromrel.cpp:95
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
virtual float getVelocityY()
Get velocity of tracked object in X direction.
virtual float getVelocityX()
Get velocity of tracked object in X direction.
virtual const char * getName() const
Get name of velocity model.
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
virtual ~VelocityGlobalFromRelative()
Destructor.
Definition: globfromrel.cpp:90
VelocityGlobalFromRelative(VelocityModel *rel_velo_model, RelativePositionModel *rel_pos_model)
Destructor.
Definition: globfromrel.cpp:47
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
virtual void setTimeNow()
Get current time from system.
Velocity model interface.
Definition: velocitymodel.h:33
virtual coordsys_type_t getCoordinateSystem()=0
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
virtual void getVelocity(float *vel_x, float *vel_y)=0
Method to retrieve velocity information.
Fawkes library namespace.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:41