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   bool get_ball_pos_relative( const char* host, HomPolar& ball_pos );
00086   bool get_ball_pos_relative( const char* host, HomPolar& ball_pos,
00087                               Matrix &ball_pos_cov );
00088   bool get_ball_pos_global(const char* host, HomPoint& ball_pos);
00089
00090   // ball velocity
00091   void set_ball_velocity( const char* from_host,
00092                           float vel_x, float vel_y, float vel_z,
00093                           float* covariance );
00094   bool get_ball_velocity( const char* from_host, HomVector& ball_vel );
00095
00096   // opponents
00097   void set_opponent_pos( const char* from_host, unsigned int uid,
00098                          float distance, float angle, float* covariance );
00099   void opponent_disappeared( const char* from_host, unsigned int uid );
00100   bool get_opponent_pos( const char* host,
00101                          std::map<unsigned int, HomPoint>& opp_positions );
00102
00103   // gamestate
00104   void set_game_state( int game_state,
00105                        worldinfo_gamestate_team_t state_team,
00106                        unsigned int score_cyan,
00107                        unsigned int score_magenta,
00108                        worldinfo_gamestate_team_t own_team,
00109                        worldinfo_gamestate_goalcolor_t own_goal_color,
00110                        worldinfo_gamestate_half_t half );
00111
00112
00113   GameState                       get_game_state() const;
00114   std::string                     get_game_state_string() const;
00115   std::string                     get_half_string() const;
00116   unsigned int                    get_own_score() const;
00117   unsigned int                    get_other_score() const;
00118   worldinfo_gamestate_team_t      get_own_team_color() const;
00119   std::string                     get_own_team_color_string() const;
00120   worldinfo_gamestate_goalcolor_t get_own_goal_color() const;
00121   std::string                     get_own_goal_color_string() const;
00122
00123
00124  private:
00125
00126   /* data structures for internal storage */
00127   /* Ball */
00128   class BallRecord
00129   {
00130   public:
00131     BallRecord();
00132     virtual ~BallRecord();
00133
00134     void set_pos( float dist, float bearing, float slope,
00135                   float* covariance = NULL );
00136     void set_visible( bool visible, int visibility_history );
00137     void set_velocity( float vel_x, float vel_y, float vel_z,
00138                        float* covariance = NULL );
00139
00140     bool      visible() const;
00141     int       visibility_history() const;
00142     HomPolar  pos_relative();
00143     HomVector vel_relative();
00144     Matrix    covariance_relative();
00145     HomPoint  pos_global( float ref_x, float ref_y, float ref_theta );
00146     HomVector vel_global( float vel_x, float vel_y, float vel_theta,
00147                           float ref_theta );
00148
00149   private:
00150     HomPolar  m_rel_pos;
00151     HomVector m_rel_vel;
00152     Matrix    m_rel_cov;
00153     bool      m_visible;
00154     int       m_visibility_history;
00155   };
00156
00157   /* Pose */
00158   class PoseRecord
00159   {
00160   public:
00161     PoseRecord();
00162     virtual ~PoseRecord();
00163
00164     void set_pose( float x, float y, float theta,
00165                    float* covariance = NULL );
00166     void set_velocity( float vel_x, float vel_y, float vel_theta,
00167                        float* covariance = NULL );
00168
00169     HomPose2d pose();
00170     Matrix    pose_covariance();
00171     HomVector velocity();
00172     Matrix    velocity_covariance();
00173
00174   private:
00175     HomPose2d m_pose;
00176     Matrix    m_pose_covariance;
00177     HomVector m_velocity;
00178     Matrix    m_velocity_covariance;
00179   };
00180
00181   /* Opponents */
00182   class OpponentsRecord
00183   {
00184   public:
00185     OpponentsRecord();
00186     virtual ~OpponentsRecord();
00187
00188     void set_pos( unsigned int opp_id, float distance, float bearing,
00189                   float* covariance = NULL );
00190     void set_pos( HomPose2d robot_pose,
00191                   unsigned int opp_id, float rel_dist, float rel_bearing,
00192                   float* rel_covariance = NULL );
00193     void disappeared( unsigned int opp_id );
00194
00195     std::map<unsigned int, HomPoint> positions();
00196
00197   private:
00198     std::map<unsigned int, HomPoint> m_glob_opp_positions;
00199   };
00200
00201   /* private methods */
00202   unsigned int get_host_id(std::string host);
00203   void         clock_in_host(unsigned int id);
00204
00205
00206   /* type definitions */
00207   typedef LockMap<std::string, unsigned int>     HostLockMap;
00208   typedef LockList<std::string>                  HostLockList;
00209   typedef LockMap<unsigned int, long>            TimeLockMap;
00210   typedef LockMap<unsigned int, BallRecord>      BallLockMap;
00211   typedef LockMap<unsigned int, PoseRecord>      PoseLockMap;
00212   typedef LockMap<unsigned int, OpponentsRecord> OpponentsLockMap;
00213
00214   /* member variables */
00215   unsigned int     m_host_id;
00216
00217   HostLockMap      m_hosts;
00218   HostLockList     m_timedout_hosts;
00219   TimeLockMap      m_last_seen;
00220   BallLockMap      m_ball_positions;
00221   PoseLockMap      m_robot_poses;
00222   OpponentsLockMap m_opponents;
00223
00224   GameState                       m_game_state;
00225   worldinfo_gamestate_team_t      m_own_team_color;
00226   worldinfo_gamestate_goalcolor_t m_own_goal_color;
00227
00228   Clock* m_clock;
00229   long   m_timeout_msec;
00230
00231   bool m_new_data_available;
00232   bool m_new_host;
00233   bool m_host_timedout;
00234
00235 };
00236
00237 } // end namespace fawkes
00238
00239 #endif /* __TOOLS_WORLDINFO_VIEWER_DATA_CONTAINER_H_ */