Fawkes API Fawkes Development Version
relvelo.h
1
2/***************************************************************************
3 * relvelo.h - A simple velocity model using the relative coordinates and
4 * robot velocity
5 *
6 * Created: Tue Oct 04 15:49:23 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#ifndef _FIREVISION_MODELS_VELOCITY_RELATIVE_H_
26#define _FIREVISION_MODELS_VELOCITY_RELATIVE_H_
27
28#include <fvmodels/relative_position/relativepositionmodel.h>
29#include <fvmodels/velocity/velocitymodel.h>
30#include <fvutils/base/types.h>
31
32// include <utils/kalman_filter/ckalman_filter_2dim.h>
33#include <list>
34
35namespace firevision {
36
37/** Position/time tuple. */
38typedef struct
39{
40 float x; /**< x pos */
41 float y; /**< y pos */
42 timeval t; /**< time */
44
45/** Velocity/time tuple. */
46typedef struct
47{
48 float vx; /**< vx in m/s */
49 float vy; /**< vy in m/s */
50 timeval t; /**< time */
52
54{
55public:
57 unsigned int max_history_length,
58 unsigned int calc_interval);
59 virtual ~VelocityFromRelative();
60
61 virtual const char *getName() const;
62
63 virtual void setRobotPosition(float x, float y, float ori, timeval t);
64 virtual void setRobotVelocity(float vel_x, float vel_y, timeval t);
65 virtual void setPanTilt(float pan, float tilt);
66 virtual void setTime(timeval t);
67 virtual void setTimeNow();
68 virtual void getTime(long int *sec, long int *usec);
69
70 virtual void getVelocity(float *vel_x, float *vel_y);
71
72 virtual float getVelocityX();
73 virtual float getVelocityY();
74
75 virtual void calc();
76 virtual void reset();
77
78 virtual coordsys_type_t getCoordinateSystem();
79
80private:
81 RelativePositionModel *relative_pos_model;
82
83 float robot_rel_vel_x;
84 float robot_rel_vel_y;
85 timeval robot_rel_vel_t;
86 timeval vel_last_time;
87
88 timeval now;
89 std::list<vel_postime_t *> ball_history;
90 std::list<vel_postime_t *>::iterator bh_it;
91
92 float f_diff_sec;
93
94 unsigned int max_history_length;
95 unsigned int calc_interval;
96
97 float cur_ball_x;
98 float cur_ball_y;
99 float cur_ball_dist;
100
101 // for projection
102 bool last_available;
103 timeval last_time;
104 float last_x;
105 float last_y;
106 float proj_x;
107 float proj_y;
108 float last_proj_error_x;
109 float last_proj_error_y;
110 float proj_time_diff_sec;
111
112 float diff_x;
113 float diff_y;
114
115 float velocity_x;
116 float velocity_y;
117
118 float avg_vx_sum;
119 float avg_vy_sum;
120 unsigned int avg_vx_num;
121 unsigned int avg_vy_num;
122
123 /*
124 bool kalman_enabled;
125 float var_proc_x;
126 float var_proc_y;
127 float var_meas_x;
128 float var_meas_y;
129 kalmanFilter2Dim *kalman_filter;
130
131 void applyKalmanFilter();
132 */
133};
134
135} // end namespace firevision
136
137#endif
Relative Position Model Interface.
Calculate velocity from relative positions.
Definition: relvelo.h:54
virtual void setPanTilt(float pan, float tilt)
Set pan and tilt.
Definition: relvelo.cpp:96
virtual void reset()
Reset velocity model Must be called if ball is not visible at any time.
Definition: relvelo.cpp:357
virtual void setRobotVelocity(float vel_x, float vel_y, timeval t)
Set robot velocity.
Definition: relvelo.cpp:106
virtual coordsys_type_t getCoordinateSystem()
Returns the used coordinate system, must be either COORDSYS_ROBOT_CART or COORDSYS_ROBOT_WORLD.
Definition: relvelo.cpp:380
virtual void setRobotPosition(float x, float y, float ori, timeval t)
Set robot position.
Definition: relvelo.cpp:101
virtual void setTime(timeval t)
Set current time.
Definition: relvelo.cpp:115
virtual float getVelocityX()
Get velocity of tracked object in X direction.
Definition: relvelo.cpp:146
virtual ~VelocityFromRelative()
Destructor.
Definition: relvelo.cpp:91
VelocityFromRelative(RelativePositionModel *model, unsigned int max_history_length, unsigned int calc_interval)
Constructor.
Definition: relvelo.cpp:46
virtual void getVelocity(float *vel_x, float *vel_y)
Method to retrieve velocity information.
Definition: relvelo.cpp:135
virtual void getTime(long int *sec, long int *usec)
Get time from velocity.
Definition: relvelo.cpp:128
virtual float getVelocityY()
Get velocity of tracked object in X direction.
Definition: relvelo.cpp:152
virtual void calc()
Calculate velocity values from given data This method must be called after all relevent data (set*) h...
Definition: relvelo.cpp:158
virtual const char * getName() const
Get name of velocity model.
Definition: relvelo.cpp:374
virtual void setTimeNow()
Get current time from system.
Definition: relvelo.cpp:122
Velocity model interface.
Definition: velocitymodel.h:33
Position/time tuple.
Definition: relvelo.h:39
Velocity/time tuple.
Definition: relvelo.h:47
float vx
vx in m/s
Definition: relvelo.h:48
float vy
vy in m/s
Definition: relvelo.h:49