00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00050 struct GameState
00051 {
00052 int game_state;
00053 worldinfo_gamestate_team_t state_team;
00054 unsigned int score_cyan;
00055 unsigned int score_magenta;
00056 worldinfo_gamestate_half_t half;
00057 };
00058
00059
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
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
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
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
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
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
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
00130
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
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
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
00210 unsigned int get_host_id(std::string host);
00211 void clock_in_host(unsigned int id);
00212
00213
00214
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
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 }
00246
00247 #endif