Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * data_container.h - World info data container 00004 * 00005 * Created: Thu April 10 16:16:17 2008 00006 * Copyright 2008 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #ifndef __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_ 00024 #define __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_ 00025 00026 #include <geometry/matrix.h> 00027 #include <geometry/hom_point.h> 00028 #include <geometry/hom_polar.h> 00029 #include <geometry/hom_pose_2d.h> 00030 #include <core/utils/lock_map.h> 00031 #include <core/utils/lock_list.h> 00032 #include <netcomm/worldinfo/enums.h> 00033 00034 #include <string> 00035 #include <list> 00036 #include <vector> 00037 #include <map> 00038 00039 namespace fawkes { 00040 00041 class Clock; 00042 00043 class WorldInfoDataContainer 00044 { 00045 public: 00046 WorldInfoDataContainer(Clock* clock, long timeout_msec = 3000); 00047 ~WorldInfoDataContainer(); 00048 00049 /** Container struct for momentary game state infos. */ 00050 struct GameState 00051 { 00052 int game_state; /**< current game state */ 00053 worldinfo_gamestate_team_t state_team; /**< team association of the game state */ 00054 unsigned int score_cyan; /**< socre of the cyan-colored team */ 00055 unsigned int score_magenta; /**< score of the magenta-colored team */ 00056 worldinfo_gamestate_half_t half; /**< first or second half */ 00057 }; 00058 00059 // management 00060 bool check_timeout(); 00061 void set_timeout(long msec); 00062 std::list<std::string> get_hosts(bool check_timeout_first = false); 00063 std::list<std::string> get_timedout_hosts(); 00064 00065 bool new_data_available(); 00066 bool new_host(); 00067 bool host_timedout(); 00068 00069 // (own) pose 00070 void set_robot_pose( const char* from_host, float x, float y, float theta, 00071 float* covariance ); 00072 bool get_robot_pose( const char* host, HomPose2d& robot_pose ); 00073 bool get_robot_pose( const char* host, HomPose2d& robot_pose, 00074 Matrix& robot_pose_cov ); 00075 00076 // (own) velocity 00077 void set_robot_velocity( const char* from_host, 00078 float vel_x, float vel_y, float vel_theta, 00079 float* covariance ); 00080 bool get_robot_velocity( const char* host, HomVector& robot_vel ); 00081 00082 // ball position 00083 void set_ball_pos( const char* from_host, bool visible, int visibility_history, 00084 float dist, float bearing, float slope, float* covariance ); 00085 void set_ball_pos_global( const char* from_host, 00086 bool visible, int visibility_history, 00087 float x, float y, float z, float* covariance ); 00088 bool get_ball_pos_relative( const char* host, HomPolar& ball_pos ); 00089 bool get_ball_pos_relative( const char* host, HomPolar& ball_pos, 00090 Matrix &ball_pos_cov ); 00091 bool get_ball_pos_global(const char* host, HomPoint& ball_pos); 00092 00093 // ball velocity 00094 void set_ball_velocity( const char* from_host, 00095 float vel_x, float vel_y, float vel_z, 00096 float* covariance ); 00097 bool get_ball_velocity( const char* from_host, HomVector& ball_vel ); 00098 00099 // opponents 00100 void set_opponent_pos( const char* from_host, unsigned int uid, 00101 float distance, float angle, float* covariance ); 00102 void opponent_disappeared( const char* from_host, unsigned int uid ); 00103 bool get_opponent_pos( const char* host, 00104 std::map<unsigned int, HomPoint>& opp_positions ); 00105 00106 // gamestate 00107 void set_game_state( int game_state, 00108 worldinfo_gamestate_team_t state_team, 00109 unsigned int score_cyan, 00110 unsigned int score_magenta, 00111 worldinfo_gamestate_team_t own_team, 00112 worldinfo_gamestate_goalcolor_t own_goal_color, 00113 worldinfo_gamestate_half_t half ); 00114 00115 00116 GameState get_game_state() const; 00117 std::string get_game_state_string() const; 00118 std::string get_half_string() const; 00119 unsigned int get_own_score() const; 00120 unsigned int get_other_score() const; 00121 worldinfo_gamestate_team_t get_own_team_color() const; 00122 std::string get_own_team_color_string() const; 00123 worldinfo_gamestate_goalcolor_t get_own_goal_color() const; 00124 std::string get_own_goal_color_string() const; 00125 00126 00127 private: 00128 /// @cond INTERNALS 00129 00130 /* data structures for internal storage */ 00131 /* Ball */ 00132 class BallRecord 00133 { 00134 public: 00135 BallRecord(); 00136 virtual ~BallRecord(); 00137 00138 void set_pos( float dist, float bearing, float slope, 00139 float* covariance = NULL ); 00140 void set_pos_global( float x, float y, float z, 00141 float* covariance = NULL ); 00142 void set_visible( bool visible, int visibility_history ); 00143 void set_velocity( float vel_x, float vel_y, float vel_z, 00144 float* covariance = NULL ); 00145 00146 bool visible() const; 00147 int visibility_history() const; 00148 HomPolar pos_relative(); 00149 HomVector vel_relative(); 00150 Matrix covariance_relative(); 00151 HomPoint pos_global(); 00152 HomPoint pos_global( float ref_x, float ref_y, float ref_theta ); 00153 HomVector vel_global( float vel_x, float vel_y, float vel_theta, 00154 float ref_theta ); 00155 00156 private: 00157 bool m_is_global; 00158 HomPolar m_rel_pos; 00159 HomVector m_rel_vel; 00160 Matrix m_rel_cov; 00161 HomPoint m_glob_pos; 00162 bool m_visible; 00163 int m_visibility_history; 00164 }; 00165 00166 /* Pose */ 00167 class PoseRecord 00168 { 00169 public: 00170 PoseRecord(); 00171 virtual ~PoseRecord(); 00172 00173 void set_pose( float x, float y, float theta, 00174 float* covariance = NULL ); 00175 void set_velocity( float vel_x, float vel_y, float vel_theta, 00176 float* covariance = NULL ); 00177 00178 HomPose2d pose(); 00179 Matrix pose_covariance(); 00180 HomVector velocity(); 00181 Matrix velocity_covariance(); 00182 00183 private: 00184 HomPose2d m_pose; 00185 Matrix m_pose_covariance; 00186 HomVector m_velocity; 00187 Matrix m_velocity_covariance; 00188 }; 00189 00190 /* Opponents */ 00191 class OpponentsRecord 00192 { 00193 public: 00194 OpponentsRecord(); 00195 virtual ~OpponentsRecord(); 00196 00197 void set_pos( unsigned int opp_id, float distance, float bearing, 00198 float* covariance = NULL ); 00199 void set_pos( HomPose2d robot_pose, 00200 unsigned int opp_id, float rel_dist, float rel_bearing, 00201 float* rel_covariance = NULL ); 00202 void disappeared( unsigned int opp_id ); 00203 00204 std::map<unsigned int, HomPoint> positions(); 00205 00206 private: 00207 std::map<unsigned int, HomPoint> m_glob_opp_positions; 00208 }; 00209 00210 /// @endcond 00211 00212 /* private methods */ 00213 unsigned int get_host_id(std::string host); 00214 void clock_in_host(unsigned int id); 00215 00216 00217 /* type definitions */ 00218 typedef LockMap<std::string, unsigned int> HostLockMap; 00219 typedef LockList<std::string> HostLockList; 00220 typedef LockMap<unsigned int, long> TimeLockMap; 00221 typedef LockMap<unsigned int, BallRecord> BallLockMap; 00222 typedef LockMap<unsigned int, PoseRecord> PoseLockMap; 00223 typedef LockMap<unsigned int, OpponentsRecord> OpponentsLockMap; 00224 00225 /* member variables */ 00226 unsigned int m_host_id; 00227 00228 HostLockMap m_hosts; 00229 HostLockList m_timedout_hosts; 00230 TimeLockMap m_last_seen; 00231 BallLockMap m_ball_positions; 00232 PoseLockMap m_robot_poses; 00233 OpponentsLockMap m_opponents; 00234 00235 GameState m_game_state; 00236 worldinfo_gamestate_team_t m_own_team_color; 00237 worldinfo_gamestate_goalcolor_t m_own_goal_color; 00238 00239 Clock* m_clock; 00240 long m_timeout_msec; 00241 00242 bool m_new_data_available; 00243 bool m_new_host; 00244 bool m_host_timedout; 00245 00246 }; 00247 00248 } // end namespace fawkes 00249 00250 #endif /* __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_ */