Fawkes API  Fawkes Development Version
data_container.h
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_ */