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 00129 /* data structures for internal storage */ 00130 /* Ball */ 00131 class BallRecord 00132 { 00133 public: 00134 BallRecord(); 00135 virtual ~BallRecord(); 00136 00137 void set_pos( float dist, float bearing, float slope, 00138 float* covariance = NULL ); 00139 void set_pos_global( float x, float y, float z, 00140 float* covariance = NULL ); 00141 void set_visible( bool visible, int visibility_history ); 00142 void set_velocity( float vel_x, float vel_y, float vel_z, 00143 float* covariance = NULL ); 00144 00145 bool visible() const; 00146 int visibility_history() const; 00147 HomPolar pos_relative(); 00148 HomVector vel_relative(); 00149 Matrix covariance_relative(); 00150 HomPoint pos_global(); 00151 HomPoint pos_global( float ref_x, float ref_y, float ref_theta ); 00152 HomVector vel_global( float vel_x, float vel_y, float vel_theta, 00153 float ref_theta ); 00154 00155 private: 00156 bool m_is_global; 00157 HomPolar m_rel_pos; 00158 HomVector m_rel_vel; 00159 Matrix m_rel_cov; 00160 HomPoint m_glob_pos; 00161 bool m_visible; 00162 int m_visibility_history; 00163 }; 00164 00165 /* Pose */ 00166 class PoseRecord 00167 { 00168 public: 00169 PoseRecord(); 00170 virtual ~PoseRecord(); 00171 00172 void set_pose( float x, float y, float theta, 00173 float* covariance = NULL ); 00174 void set_velocity( float vel_x, float vel_y, float vel_theta, 00175 float* covariance = NULL ); 00176 00177 HomPose2d pose(); 00178 Matrix pose_covariance(); 00179 HomVector velocity(); 00180 Matrix velocity_covariance(); 00181 00182 private: 00183 HomPose2d m_pose; 00184 Matrix m_pose_covariance; 00185 HomVector m_velocity; 00186 Matrix m_velocity_covariance; 00187 }; 00188 00189 /* Opponents */ 00190 class OpponentsRecord 00191 { 00192 public: 00193 OpponentsRecord(); 00194 virtual ~OpponentsRecord(); 00195 00196 void set_pos( unsigned int opp_id, float distance, float bearing, 00197 float* covariance = NULL ); 00198 void set_pos( HomPose2d robot_pose, 00199 unsigned int opp_id, float rel_dist, float rel_bearing, 00200 float* rel_covariance = NULL ); 00201 void disappeared( unsigned int opp_id ); 00202 00203 std::map<unsigned int, HomPoint> positions(); 00204 00205 private: 00206 std::map<unsigned int, HomPoint> m_glob_opp_positions; 00207 }; 00208 00209 /* private methods */ 00210 unsigned int get_host_id(std::string host); 00211 void clock_in_host(unsigned int id); 00212 00213 00214 /* type definitions */ 00215 typedef LockMap<std::string, unsigned int> HostLockMap; 00216 typedef LockList<std::string> HostLockList; 00217 typedef LockMap<unsigned int, long> TimeLockMap; 00218 typedef LockMap<unsigned int, BallRecord> BallLockMap; 00219 typedef LockMap<unsigned int, PoseRecord> PoseLockMap; 00220 typedef LockMap<unsigned int, OpponentsRecord> OpponentsLockMap; 00221 00222 /* member variables */ 00223 unsigned int m_host_id; 00224 00225 HostLockMap m_hosts; 00226 HostLockList m_timedout_hosts; 00227 TimeLockMap m_last_seen; 00228 BallLockMap m_ball_positions; 00229 PoseLockMap m_robot_poses; 00230 OpponentsLockMap m_opponents; 00231 00232 GameState m_game_state; 00233 worldinfo_gamestate_team_t m_own_team_color; 00234 worldinfo_gamestate_goalcolor_t m_own_goal_color; 00235 00236 Clock* m_clock; 00237 long m_timeout_msec; 00238 00239 bool m_new_data_available; 00240 bool m_new_host; 00241 bool m_host_timedout; 00242 00243 }; 00244 00245 } // end namespace fawkes 00246 00247 #endif /* __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_ */