Fawkes API  Fawkes Development Version
data_container.h
1 
2 /***************************************************************************
3  * data_container.h - World info data container
4  *
5  * Created: Thu April 10 16:16:17 2008
6  * Copyright 2008 Daniel Beck
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_
24 #define __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_
25 
26 #include <geometry/matrix.h>
27 #include <geometry/hom_point.h>
28 #include <geometry/hom_polar.h>
29 #include <geometry/hom_pose_2d.h>
30 #include <core/utils/lock_map.h>
31 #include <core/utils/lock_list.h>
32 #include <netcomm/worldinfo/enums.h>
33 
34 #include <string>
35 #include <list>
36 #include <vector>
37 #include <map>
38 
39 namespace fawkes {
40 
41 class Clock;
42 
44 {
45  public:
46  WorldInfoDataContainer(Clock* clock, long timeout_msec = 3000);
48 
49  /** Container struct for momentary game state infos. */
50  struct GameState
51  {
52  int game_state; /**< current game state */
53  worldinfo_gamestate_team_t state_team; /**< team association of the game state */
54  unsigned int score_cyan; /**< socre of the cyan-colored team */
55  unsigned int score_magenta; /**< score of the magenta-colored team */
56  worldinfo_gamestate_half_t half; /**< first or second half */
57  };
58 
59  // management
60  bool check_timeout();
61  void set_timeout(long msec);
62  std::list<std::string> get_hosts(bool check_timeout_first = false);
63  std::list<std::string> get_timedout_hosts();
64 
65  bool new_data_available();
66  bool new_host();
67  bool host_timedout();
68 
69  // (own) pose
70  void set_robot_pose( const char* from_host, float x, float y, float theta,
71  float* covariance );
72  bool get_robot_pose( const char* host, HomPose2d& robot_pose );
73  bool get_robot_pose( const char* host, HomPose2d& robot_pose,
74  Matrix& robot_pose_cov );
75 
76  // (own) velocity
77  void set_robot_velocity( const char* from_host,
78  float vel_x, float vel_y, float vel_theta,
79  float* covariance );
80  bool get_robot_velocity( const char* host, HomVector& robot_vel );
81 
82  // ball position
83  void set_ball_pos( const char* from_host, bool visible, int visibility_history,
84  float dist, float bearing, float slope, float* covariance );
85  void set_ball_pos_global( const char* from_host,
86  bool visible, int visibility_history,
87  float x, float y, float z, float* covariance );
88  bool get_ball_pos_relative( const char* host, HomPolar& ball_pos );
89  bool get_ball_pos_relative( const char* host, HomPolar& ball_pos,
90  Matrix &ball_pos_cov );
91  bool get_ball_pos_global(const char* host, HomPoint& ball_pos);
92 
93  // ball velocity
94  void set_ball_velocity( const char* from_host,
95  float vel_x, float vel_y, float vel_z,
96  float* covariance );
97  bool get_ball_velocity( const char* from_host, HomVector& ball_vel );
98 
99  // opponents
100  void set_opponent_pos( const char* from_host, unsigned int uid,
101  float distance, float angle, float* covariance );
102  void opponent_disappeared( const char* from_host, unsigned int uid );
103  bool get_opponent_pos( const char* host,
104  std::map<unsigned int, HomPoint>& opp_positions );
105 
106  // gamestate
107  void set_game_state( int game_state,
108  worldinfo_gamestate_team_t state_team,
109  unsigned int score_cyan,
110  unsigned int score_magenta,
112  worldinfo_gamestate_goalcolor_t own_goal_color,
114 
115 
116  GameState get_game_state() const;
117  std::string get_game_state_string() const;
118  std::string get_half_string() const;
119  unsigned int get_own_score() const;
120  unsigned int get_other_score() const;
122  std::string get_own_team_color_string() const;
124  std::string get_own_goal_color_string() const;
125 
126 
127  private:
128  /// @cond INTERNALS
129 
130  /* data structures for internal storage */
131  /* Ball */
132  class BallRecord
133  {
134  public:
135  BallRecord();
136  virtual ~BallRecord();
137 
138  void set_pos( float dist, float bearing, float slope,
139  float* covariance = NULL );
140  void set_pos_global( float x, float y, float z,
141  float* covariance = NULL );
142  void set_visible( bool visible, int visibility_history );
143  void set_velocity( float vel_x, float vel_y, float vel_z,
144  float* covariance = NULL );
145 
146  bool visible() const;
147  int visibility_history() const;
148  HomPolar pos_relative();
149  HomVector vel_relative();
150  Matrix covariance_relative();
151  HomPoint pos_global();
152  HomPoint pos_global( float ref_x, float ref_y, float ref_theta );
153  HomVector vel_global( float vel_x, float vel_y, float vel_theta,
154  float ref_theta );
155 
156  private:
157  bool m_is_global;
158  HomPolar m_rel_pos;
159  HomVector m_rel_vel;
160  Matrix m_rel_cov;
161  HomPoint m_glob_pos;
162  bool m_visible;
163  int m_visibility_history;
164  };
165 
166  /* Pose */
167  class PoseRecord
168  {
169  public:
170  PoseRecord();
171  virtual ~PoseRecord();
172 
173  void set_pose( float x, float y, float theta,
174  float* covariance = NULL );
175  void set_velocity( float vel_x, float vel_y, float vel_theta,
176  float* covariance = NULL );
177 
178  HomPose2d pose();
179  Matrix pose_covariance();
180  HomVector velocity();
181  Matrix velocity_covariance();
182 
183  private:
184  HomPose2d m_pose;
185  Matrix m_pose_covariance;
186  HomVector m_velocity;
187  Matrix m_velocity_covariance;
188  };
189 
190  /* Opponents */
191  class OpponentsRecord
192  {
193  public:
194  OpponentsRecord();
195  virtual ~OpponentsRecord();
196 
197  void set_pos( unsigned int opp_id, float distance, float bearing,
198  float* covariance = NULL );
199  void set_pos( HomPose2d robot_pose,
200  unsigned int opp_id, float rel_dist, float rel_bearing,
201  float* rel_covariance = NULL );
202  void disappeared( unsigned int opp_id );
203 
204  std::map<unsigned int, HomPoint> positions();
205 
206  private:
207  std::map<unsigned int, HomPoint> m_glob_opp_positions;
208  };
209 
210  /// @endcond
211 
212  /* private methods */
213  unsigned int get_host_id(std::string host);
214  void clock_in_host(unsigned int id);
215 
216 
217  /* type definitions */
218  typedef LockMap<std::string, unsigned int> HostLockMap;
219  typedef LockList<std::string> HostLockList;
220  typedef LockMap<unsigned int, long> TimeLockMap;
221  typedef LockMap<unsigned int, BallRecord> BallLockMap;
222  typedef LockMap<unsigned int, PoseRecord> PoseLockMap;
223  typedef LockMap<unsigned int, OpponentsRecord> OpponentsLockMap;
224 
225  /* member variables */
226  unsigned int m_host_id;
227 
228  HostLockMap m_hosts;
229  HostLockList m_timedout_hosts;
230  TimeLockMap m_last_seen;
231  BallLockMap m_ball_positions;
232  PoseLockMap m_robot_poses;
233  OpponentsLockMap m_opponents;
234 
235  GameState m_game_state;
236  worldinfo_gamestate_team_t m_own_team_color;
237  worldinfo_gamestate_goalcolor_t m_own_goal_color;
238 
239  Clock* m_clock;
240  long m_timeout_msec;
241 
242  bool m_new_data_available;
243  bool m_new_host;
244  bool m_host_timedout;
245 
246 };
247 
248 } // end namespace fawkes
249 
250 #endif /* __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_ */
Data container to store and exchange worldinfo data.
bool host_timedout()
Check whether a host has timed out.
bool get_ball_velocity(const char *from_host, HomVector &ball_vel)
Obtain ball velocity information for specified robot.
void set_timeout(long msec)
Set the time out.
GameState get_game_state() const
Obtain the game state.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62
void set_ball_velocity(const char *from_host, float vel_x, float vel_y, float vel_z, float *covariance)
Set the ball velocity as it is estimated by the specified robot.
unsigned int get_own_score() const
Get own score.
unsigned int score_magenta
score of the magenta-colored team
A homogeneous representation of a polar coordinate.
Definition: hom_polar.h:31
Fawkes library namespace.
void set_ball_pos_global(const char *from_host, bool visible, int visibility_history, float x, float y, float z, float *covariance)
Set the global ball position estimation of a robot.
A general matrix class.
Definition: matrix.h:33
This is supposed to be the central clock in Fawkes.
Definition: clock.h:34
bool check_timeout()
Check for timed out hosts.
A 2-dimensional pose, i.e.
Definition: hom_pose_2d.h:33
worldinfo_gamestate_team_t get_own_team_color() const
Get own team color.
WorldInfoDataContainer(Clock *clock, long timeout_msec=3000)
Constructor.
void opponent_disappeared(const char *from_host, unsigned int uid)
Remove the opponent with the given ID form the list of opponents seen by the given robot...
std::list< std::string > get_hosts(bool check_timeout_first=false)
Obtain the list of active hosts.
void set_game_state(int game_state, worldinfo_gamestate_team_t state_team, unsigned int score_cyan, unsigned int score_magenta, worldinfo_gamestate_team_t own_team, worldinfo_gamestate_goalcolor_t own_goal_color, worldinfo_gamestate_half_t half)
Set the gamestate.
std::string get_own_goal_color_string() const
Get own goal color as string.
bool new_data_available()
Check whehter new data is available.
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:70
worldinfo_gamestate_goalcolor_t get_own_goal_color() const
Get own goal color.
std::string get_game_state_string() const
Get the current game state as string.
A homogeneous point.
Definition: hom_point.h:33
std::string get_half_string() const
Get the current half as string.
worldinfo_gamestate_team_t state_team
team association of the game state
std::list< std::string > get_timedout_hosts()
Obtain the list of timedout hosts.
bool get_opponent_pos(const char *host, std::map< unsigned int, HomPoint > &opp_positions)
Get all oppenents detected by a certain robot.
worldinfo_gamestate_half_t half
first or second half
unsigned int score_cyan
socre of the cyan-colored team
bool get_robot_pose(const char *host, HomPose2d &robot_pose)
Obtain the pose of the given robot.
bool get_ball_pos_global(const char *host, HomPoint &ball_pos)
Get the global position of the ball as it is estimated by the specified robot.
unsigned int get_other_score() const
Get score of the other team.
A homogeneous vector.
Definition: hom_vector.h:31
void set_opponent_pos(const char *from_host, unsigned int uid, float distance, float angle, float *covariance)
Set the position of a detected opponent.
std::string get_own_team_color_string() const
Get own team color as string.
void set_robot_velocity(const char *from_host, float vel_x, float vel_y, float vel_theta, float *covariance)
Set the velocity of the robot.
worldinfo_gamestate_team_t
Team.
Definition: enums.h:54
void set_robot_pose(const char *from_host, float x, float y, float theta, float *covariance)
Set the pose of a robot.
Container struct for momentary game state infos.
void set_ball_pos(const char *from_host, bool visible, int visibility_history, float dist, float bearing, float slope, float *covariance)
Set the ball position estimation of a robot.
bool get_robot_velocity(const char *host, HomVector &robot_vel)
Obtain current velocity of the specified robot.
bool get_ball_pos_relative(const char *host, HomPolar &ball_pos)
Get the ball position estimation of a certain robot.
bool new_host()
Check whether a new host has been added recently.
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:63