00001
00002 /***************************************************************************
00003  *  data_container.cpp - World info data container
00004  *
00005  *  Created: Thu April 10 22:23:27 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 #include <worldinfo_utils/data_container.h>
00024 #include <utils/time/clock.h>
00025 #include <utils/time/time.h>
00026 #include <cstdlib>
00027 #include <cstdio>
00028 #include <cmath>
00029
00030 using namespace std;
00031 namespace fawkes {
00032
00033 WorldInfoDataContainer::BallRecord::BallRecord()
00034 {
00035 }
00036
00037 WorldInfoDataContainer::BallRecord::~BallRecord()
00038 {
00039 }
00040
00041 void
00042 WorldInfoDataContainer::BallRecord::set_pos( float dist,
00043                                              float bearing,
00044                                              float slope,
00045                                              float* covariance )
00046 {
00047   m_rel_pos.r(dist);
00048   m_rel_pos.phi(bearing);
00049   // TODO: slope, covariance
00050 }
00051
00052 void
00053 WorldInfoDataContainer::BallRecord::set_visible( bool visible,
00054                                                  int visibility_history )
00055 {
00056   m_visible = visible;
00057   m_visibility_history = visibility_history;
00058 }
00059
00060 void
00061 WorldInfoDataContainer::BallRecord::set_velocity( float vel_x,
00062                                                   float vel_y,
00063                                                   float vel_z,
00064                                                   float* covariance )
00065 {
00066   m_rel_vel.x(vel_x);
00067   m_rel_vel.y(vel_y);
00068   m_rel_vel.z(vel_z);
00069   // TODO: covariance
00070 }
00071
00072 bool
00073 WorldInfoDataContainer::BallRecord::visible() const
00074 {
00075   return m_visible;
00076 }
00077
00078 int
00079 WorldInfoDataContainer::BallRecord::visibility_history() const
00080 {
00081   return m_visibility_history;
00082 }
00083
00084 HomPolar
00085 WorldInfoDataContainer::BallRecord::pos_relative()
00086 {
00087   return m_rel_pos;
00088 }
00089
00090 HomVector
00091 WorldInfoDataContainer::BallRecord::vel_relative()
00092 {
00093   return m_rel_vel;
00094 }
00095
00096 Matrix
00097 WorldInfoDataContainer::BallRecord::covariance_relative()
00098 {
00099   return m_rel_cov;
00100 }
00101
00102 HomPoint
00103 WorldInfoDataContainer::BallRecord::pos_global( float ref_x,
00104                                                 float ref_y,
00105                                                 float ref_theta )
00106 {
00107   HomPoint p( m_rel_pos.x(), m_rel_pos.y() );
00108   p.rotate_z( ref_theta );
00109   p.x() += ref_x;
00110   p.y() += ref_y;
00111   return p;
00112 }
00113
00114 HomVector
00115 WorldInfoDataContainer::BallRecord::vel_global( float vel_x,
00116                                                 float vel_y,
00117                                                 float vel_theta,
00118                                                 float ref_theta )
00119 {
00120   // TODO
00121   return HomVector(0.0, 0.0, 0.0);
00122 }
00123
00124 WorldInfoDataContainer::PoseRecord::PoseRecord()
00125 {
00126 }
00127
00128 WorldInfoDataContainer::PoseRecord::~PoseRecord()
00129 {
00130 }
00131
00132 void
00133 WorldInfoDataContainer::PoseRecord::set_pose( float x,
00134                                               float y,
00135                                               float theta,
00136                                               float* covariance )
00137 {
00138   m_pose.x( x );
00139   m_pose.y( y );
00140   m_pose.yaw( theta );
00141   // TODO: covariance
00142 }
00143
00144 void
00145 WorldInfoDataContainer::PoseRecord::set_velocity( float vel_x,
00146                                                   float vel_y,
00147                                                   float vel_theta,
00148                                                   float* covariance )
00149 {
00150   m_velocity.x() = vel_x;
00151   m_velocity.y() = vel_y;
00152   m_velocity.z() = vel_theta;
00153   // TODO: covariance
00154 }
00155
00156 HomPose2d
00157 WorldInfoDataContainer::PoseRecord::pose()
00158 {
00159   return m_pose;
00160 }
00161
00162 Matrix
00163 WorldInfoDataContainer::PoseRecord::pose_covariance()
00164 {
00165   return m_pose_covariance;
00166 }
00167
00168 HomVector
00169 WorldInfoDataContainer::PoseRecord::velocity()
00170 {
00171   return m_velocity;
00172 }
00173
00174 WorldInfoDataContainer::OpponentsRecord::OpponentsRecord()
00175 {
00176 }
00177
00178 WorldInfoDataContainer::OpponentsRecord::~OpponentsRecord()
00179 {
00180 }
00181
00182 void
00183 WorldInfoDataContainer::OpponentsRecord::set_pos( unsigned int id,
00184                                                   float distance,
00185                                                   float bearing,
00186                                                   float* covariance )
00187 {
00188   // TODO
00189 }
00190
00191 void
00192 WorldInfoDataContainer::OpponentsRecord::set_pos( HomPose2d robot_pose,
00193                                                   unsigned int opp_id,
00194                                                   float rel_distance,
00195                                                   float rel_bearing,
00196                                                   float* rel_covariance )
00197 {
00198   HomTransform local_to_global;
00199   local_to_global.rotate_z( robot_pose.yaw() );
00200   local_to_global.trans( robot_pose.x(), robot_pose.y() );
00201   HomPoint o = local_to_global * HomPoint( cos( rel_bearing ) * rel_distance,
00202                                            sin( rel_bearing ) * rel_distance );
00203
00204   m_glob_opp_positions[ opp_id ] = o;
00205
00206   // TODO: covariance
00207 }
00208
00209 void
00210 WorldInfoDataContainer::OpponentsRecord::disappeared( unsigned int opp_id )
00211 {
00212   m_glob_opp_positions.erase( opp_id );
00213 }
00214
00215 map<unsigned int, HomPoint>
00216 WorldInfoDataContainer::OpponentsRecord::positions()
00217 {
00218   return m_glob_opp_positions;
00219 }
00220
00221 
00222 /** @class WorldInfoDataContainer <worldinfo_utils/data_container.h>
00223  * Data container to store and exchange worldinfo data.
00224  * @author Daniel Beck
00225  */
00226 
00227 /** Constructor.
00228  * @param clock pointer to a Clock
00229  * @param timeout_msec timeout in milliseconds
00230  */
00231 WorldInfoDataContainer::WorldInfoDataContainer( Clock* clock,
00232                                                 long timeout_msec )
00233 {
00234   m_clock        = clock;
00235   m_timeout_msec = timeout_msec;
00236
00237   m_host_id = 0;
00238
00239   m_own_team_color = TEAM_CYAN;
00240   m_own_goal_color = GOAL_BLUE;
00241
00242   m_game_state.game_state    = GS_FROZEN;
00243   m_game_state.state_team    = TEAM_BOTH;
00244   m_game_state.score_cyan    = 0;
00245   m_game_state.score_magenta = 0;
00246   m_game_state.half          = HALF_FIRST;
00247
00248   m_new_data_available = false;
00249   m_new_host           = false;
00250   m_host_timedout      = false;
00251 }
00252 
00253 /** Destructor. */
00254 WorldInfoDataContainer::~WorldInfoDataContainer()
00255 {
00256 }
00257 
00258 /** Check for timed out hosts.
00259  * This method should be called regularly to remove hosts from the
00260  * data container from which no data has been received in a certain
00261  * amount of time.
00262  * @return true if there are timed out hosts
00263  */
00264 bool
00265 WorldInfoDataContainer::check_timeout()
00266 {
00267   Time now(m_clock);
00268   now.stamp();
00269
00270   m_timedout_hosts.lock();
00271   m_timedout_hosts.clear();
00272   m_timedout_hosts.unlock();
00273
00274   m_hosts.lock();
00275   HostLockMap::iterator iter = m_hosts.begin();
00276   while ( iter != m_hosts.end() )
00277     {
00278       unsigned int id = iter->second;
00279
00280       if ( now.in_msec() - m_last_seen[id] < m_timeout_msec )
00281         { ++iter; }
00282       else
00283         {
00284           m_last_seen.lock();
00285           m_last_seen.erase(id);
00286           m_last_seen.unlock();
00287
00288           m_ball_positions.lock();
00289           m_ball_positions.erase(id);
00290           m_ball_positions.unlock();
00291
00292           m_robot_poses.lock();
00293           m_robot_poses.erase(id);
00294           m_robot_poses.unlock();
00295
00296           m_timedout_hosts.lock();
00297           m_timedout_hosts.push_back(iter->first);
00298           m_timedout_hosts.unlock();
00299
00300           m_hosts.erase(iter++);
00301           m_host_timedout = true;
00302         }
00303     }
00304
00305   m_hosts.unlock();
00306
00307   return m_timedout_hosts.size() != 0;
00308 }
00309 
00310 /** Set the time out.
00311  * @param msec time out value in milliseconds
00312  */
00313 void
00314 WorldInfoDataContainer::set_timeout(long msec)
00315 {
00316   m_timeout_msec = msec;
00317 }
00318 
00319 /** Obtain the list of active hosts.
00320  * @param check_timeout_first if true check_timeout() is called before
00321  * the list is compiled
00322  * @return the list of active hosts
00323  */
00324 std::list<std::string>
00325 WorldInfoDataContainer::get_hosts(bool check_timeout_first)
00326 {
00327   if (check_timeout_first)
00328     { check_timeout(); }
00329
00330   list<string> hosts;
00331
00332   m_hosts.lock();
00333   for ( HostLockMap::iterator iter = m_hosts.begin();
00334         iter != m_hosts.end();
00335         ++iter )
00336     { hosts.push_back( iter->first ); }
00337   m_hosts.unlock();
00338
00339   return hosts;
00340 }
00341 
00342 /** Obtain the list of timedout hosts.
00343  * Hosts that have been marked as timedout in the last call of
00344  * check_timeout().
00345  * @return the list of timedout hosts
00346  */
00347 std::list<std::string>
00348 WorldInfoDataContainer::get_timedout_hosts()
00349 {
00350   list<string> timedout_hosts;
00351
00352   m_timedout_hosts.lock();
00353   for ( HostLockList::iterator iter = m_timedout_hosts.begin();
00354         iter != m_timedout_hosts.end();
00355         ++iter )
00356     { timedout_hosts.push_back( *iter ); }
00357   m_timedout_hosts.unlock();
00358
00359   return timedout_hosts;
00360 }
00361 
00362 /** Check whehter new data is available.
00363  * @return true if new data is available.
00364  */
00365 bool
00366 WorldInfoDataContainer::new_data_available()
00367 {
00368   bool new_data = m_new_data_available;
00369   m_new_data_available = false;
00370   return new_data;
00371 }
00372 
00373 /** Check whether a new host has been added recently.
00374  * @return true if a new host has been added recently
00375  */
00376 bool
00377 WorldInfoDataContainer::new_host()
00378 {
00379   bool new_host = m_new_host;
00380   m_new_host = false;
00381   return new_host;
00382 }
00383 
00384 /** Check whether a host has timed out.
00385  * @return true if a host has timed out recently
00386  */
00387 bool
00388 WorldInfoDataContainer::host_timedout()
00389 {
00390   bool host_timedout = m_host_timedout;
00391   m_host_timedout = false;
00392   return host_timedout;
00393 }
00394
00395 
00396 /** Set the pose of a robot.
00397  * @param host the hostname of the robot
00398  * @param x the x-coordinate of the robot's global position
00399  * @param y the y-coordinate of the robot's global position
00400  * @param theta the global orientation of the robot
00401  * @param covariance covariance associated with the position
00402  * estimation
00403  */
00404 void
00405 WorldInfoDataContainer::set_robot_pose( const char* host,
00406                                         float x,
00407                                         float y,
00408                                         float theta,
00409                                         float* covariance )
00410 {
00411   PoseLockMap::iterator iter;
00412   unsigned int id = get_host_id( host );
00413   clock_in_host( id );
00414
00415   m_robot_poses.lock();
00416   iter = m_robot_poses.find( id );
00417   if ( iter == m_robot_poses.end() )
00418     {
00419       PoseRecord pose_record;
00420       pose_record.set_pose( x, y, theta, covariance );
00421       m_robot_poses[ id ] = pose_record;
00422     }
00423   else
00424     {
00425       iter->second.set_pose( x, y, theta, covariance );
00426     }
00427   m_robot_poses.unlock();
00428
00429   m_new_data_available = true;
00430 }
00431 
00432 /** Obtain the pose of the given robot.
00433  * @param host the hostname of the robot
00434  * @param pose reference to a HomPose where the pose will be stored
00435  * @return false if no pose for the requested robot could be found
00436  */
00437 bool
00438 WorldInfoDataContainer::get_robot_pose( const char* host,
00439                                         HomPose2d& pose )
00440 {
00441   bool found = false;
00442   unsigned int id = get_host_id( host );
00443
00444   m_robot_poses.lock();
00445   PoseLockMap::iterator iter = m_robot_poses.find( id );
00446
00447   if ( iter != m_robot_poses.end() )
00448     {
00449       pose = iter->second.pose();
00450       found = true;
00451     }
00452   m_robot_poses.unlock();
00453
00454   return found;
00455 }
00456
00457 
00458 /** Get the position of a certain robot.
00459  * @param host the hostname of the robot
00460  * @param pose reference to a HomPoint where the global position of
00461  * the robot is written to
00462  * @param pose_cov reference to a Matrix where the covariance of the
00463  * robot position is written to
00464  * @return true if a pose for the robot could be found
00465  */
00466 bool
00467 WorldInfoDataContainer::get_robot_pose( const char* host,
00468                                         HomPose2d& pose,
00469                                         Matrix& pose_cov )
00470 {
00471   bool found = false;
00472   unsigned int id = get_host_id( host );
00473
00474   m_robot_poses.lock();
00475   PoseLockMap::iterator iter = m_robot_poses.find( id );
00476
00477   if ( iter != m_robot_poses.end() )
00478     {
00479       pose = iter->second.pose();
00480       pose_cov = iter->second.pose_covariance();
00481       found = true;
00482     }
00483   m_robot_poses.unlock();
00484
00485   return found;
00486 }
00487
00488 
00489 /** Set the velocity of the robot.
00490  * @param host the hostname of the robot
00491  * @param vel_x the current forward velocity of the robot
00492  * @param vel_y the current sideward velocity of the robot
00493  * @param vel_theta the current rotational velociy of the robot
00494  * @param covariance the velocity covariance
00495  */
00496 void
00497 WorldInfoDataContainer::set_robot_velocity( const char* host,
00498                                             float vel_x,
00499                                             float vel_y,
00500                                             float vel_theta,
00501                                             float* covariance )
00502 {
00503   PoseLockMap::iterator iter;
00504   unsigned int id = get_host_id( host );
00505   clock_in_host( id );
00506
00507   m_robot_poses.lock();
00508   iter = m_robot_poses.find( id );
00509   if ( iter == m_robot_poses.end() )
00510     {
00511       PoseRecord pose_record;
00512       pose_record.set_velocity( vel_x, vel_y, vel_theta, covariance );
00513       m_robot_poses[ id ] = pose_record;
00514     }
00515   else
00516     {
00517       iter->second.set_velocity( vel_x, vel_y, vel_theta, covariance );
00518     }
00519   m_robot_poses.unlock();
00520
00521   m_new_data_available = true;
00522 }
00523
00524 
00525 /** Obtain current velocity of the specified robot.
00526  * @param host the hostname of the robot
00527  * @param robot_vel reference to a HomVector where the velocity
00528  * information is written to
00529  * @return true, if velocity information for the specified host are
00530  * available
00531  */
00532 bool
00533 WorldInfoDataContainer::get_robot_velocity( const char* host,
00534                                             HomVector& robot_vel )
00535 {
00536   // TODO
00537   return true;
00538 }
00539
00540 
00541 /** Set the ball position estimation of a robot.
00542  * @param host the hostname of the robot
00543  * @param visible visible or not
00544  * @param visibility_history visible/not visible for n iterations
00545  * @param dist distance to the robot
00546  * @param bearing vertical angle to the ball
00547  * @param slope the horizontal angle to the ball
00548  * @param covariance covariance associated with the position estimation
00549  */
00550 void
00551 WorldInfoDataContainer::set_ball_pos( const char* host,
00552                                       bool visible,
00553                                       int visibility_history,
00554                                       float dist,
00555                                       float bearing,
00556                                       float slope,
00557                                       float* covariance )
00558 {
00559   BallLockMap::iterator iter;
00560   unsigned int id = get_host_id( host );
00561   clock_in_host( id );
00562
00563   m_ball_positions.lock();
00564   iter = m_ball_positions.find( id );
00565   if ( iter == m_ball_positions.end() )
00566     {
00567       BallRecord ball_record;
00568       ball_record.set_visible( visible, visibility_history );
00569       ball_record.set_pos( dist, bearing, slope, covariance );
00570       m_ball_positions[ id ] = ball_record;
00571     }
00572   else
00573     {
00574       iter->second.set_visible( visible, visibility_history );
00575       iter->second.set_pos( dist, bearing, slope, covariance );
00576     }
00577   m_ball_positions.unlock();
00578
00579   m_new_data_available = true;
00580 }
00581
00582 
00583 /** Get the ball position estimation of a certain robot.
00584  * @param host the hostname of the robot
00585  * @param pos reference to a HomPolar where the position is written to
00586  * @return true if a global ball position was found
00587  */
00588 bool
00589 WorldInfoDataContainer::get_ball_pos_relative( const char* host,
00590                                                HomPolar& pos )
00591 {
00592   bool found = false;
00593   unsigned int id = get_host_id( host );
00594
00595   m_ball_positions.lock();
00596   BallLockMap::iterator iter = m_ball_positions.find( id );
00597
00598   if ( iter != m_ball_positions.end() )
00599     {
00600       pos = iter->second.pos_relative();
00601       found = iter->second.visible();
00602     }
00603   m_ball_positions.unlock();
00604
00605   return found;
00606 }
00607
00608 
00609 /** Get the ball position estimation of a certain robot.
00610  * @param host the hostname of the robot
00611  * @param pos reference to a HomPolar where the position is written to
00612  * @param pos_cov reference to a Matrix where the ball position
00613  * covariance is written to
00614  * @return true if a global ball position was found
00615  */
00616 bool
00617 WorldInfoDataContainer::get_ball_pos_relative( const char* host,
00618                                                HomPolar& pos,
00619                                                Matrix& pos_cov )
00620 {
00621   bool found = false;
00622   unsigned int id = get_host_id( host );
00623
00624   m_ball_positions.lock();
00625   BallLockMap::iterator iter = m_ball_positions.find( id );
00626
00627   if ( iter != m_ball_positions.end() )
00628     {
00629       pos = iter->second.pos_relative();
00630       pos_cov = iter->second.covariance_relative();
00631       found = iter->second.visible();
00632     }
00633   m_ball_positions.unlock();
00634
00635   return found;
00636 }
00637
00638 
00639 /** Get the global position of the ball as it is estimated by the
00640  * specified robot.
00641  * @param host the robot's hostname
00642  * @param pos refercence to a HomPoint where the position of the ball
00643  * written to
00644  */
00645 bool
00646 WorldInfoDataContainer::get_ball_pos_global( const char* host,
00647                                              HomPoint& pos )
00648 {
00649   bool found = false;
00650   unsigned int id = get_host_id( host );
00651
00652   m_ball_positions.lock();
00653   m_robot_poses.lock();
00654   BallLockMap::iterator ball_iter = m_ball_positions.find( id );
00655   PoseLockMap::iterator pose_iter = m_robot_poses.find( id );
00656
00657   if ( ball_iter != m_ball_positions.end() &&
00658        pose_iter != m_robot_poses.end() )
00659     {
00660       HomPose2d robot_pose = pose_iter->second.pose();
00661       pos = ball_iter->second.pos_global( robot_pose.x(),
00662                                           robot_pose.y(),
00663                                           robot_pose.yaw() );
00664       found = ball_iter->second.visible();
00665     }
00666   m_robot_poses.unlock();
00667   m_ball_positions.unlock();
00668
00669   return found;
00670 }
00671
00672 
00673 /** Set the ball velocity as it is estimated by the specified robot.
00674  * @param host the hostname of the robot
00675  * @param vel_x the ball velocity in x-direction of the robot-centered
00676  * coordinate system
00677  * @param vel_y the ball velocity in y-direction of the robot-centered
00678  * coordinate system
00679  * @param vel_z the ball velocity in z-direction of the robot-centered
00680  * coordinate system
00681  * @param covariance ball velocity covariance
00682  */
00683 void
00684 WorldInfoDataContainer::set_ball_velocity( const char* host,
00685                                            float vel_x,
00686                                            float vel_y,
00687                                            float vel_z,
00688                                            float* covariance )
00689 {
00690   BallLockMap::iterator iter;
00691   unsigned int id = get_host_id( host );
00692   clock_in_host( id );
00693
00694   m_ball_positions.lock();
00695   iter = m_ball_positions.find( id );
00696   if ( iter == m_ball_positions.end() )
00697     {
00698       BallRecord ball_record;
00699       ball_record.set_velocity( vel_x, vel_y, vel_z, covariance );
00700       m_ball_positions[ id ] = ball_record;
00701     }
00702   else
00703     {
00704       iter->second.set_velocity( vel_x, vel_y, vel_z, covariance );
00705     }
00706   m_ball_positions.unlock();
00707
00708   m_new_data_available = true;
00709 }
00710
00711 
00712 /** Obtain ball velocity information for specified robot.
00713  * @param host the hostname of the robot
00714  * @param ball_vel refrence to a HomVector where the velocity
00715  * information is written to
00716  * @return true if ball velocity information from the specified robot
00717  * are available
00718  */
00719 bool
00720 WorldInfoDataContainer::get_ball_velocity( const char* host,
00721                                            HomVector& ball_vel )
00722 {
00723   // TODO
00724   return true;
00725 }
00726 
00727 /** Set the position of a detected opponent.
00728  * @param host hostname of the robot that detected the robot
00729  * @param uid opponent id
00730  * @param distance distance to the robot
00731  * @param angle angle at which the opponent is detected
00732  * @param covariance corresponding covariance matrix
00733  */
00734 void
00735 WorldInfoDataContainer::set_opponent_pos( const char* host,
00736                                           unsigned int uid,
00737                                           float distance,
00738                                           float angle,
00739                                           float* covariance )
00740 {
00741   unsigned int id = get_host_id( host );
00742   clock_in_host( id );
00743
00744   m_opponents.lock();
00745   m_robot_poses.lock();
00746   OpponentsLockMap::iterator oit = m_opponents.find( id );
00747   PoseLockMap::iterator      pit = m_robot_poses.find( id );
00748
00749   HomPose2d pose;
00750   if ( pit != m_robot_poses.end() )
00751   { pose = pit->second.pose(); }
00752
00753   if ( oit == m_opponents.end() )
00754   {
00755     OpponentsRecord opponents_record;
00756     opponents_record.set_pos( pose, uid, distance, angle, covariance );
00757     m_opponents[ id ] = opponents_record;
00758   }
00759   else
00760   {
00761     oit->second.set_pos( pose, uid, distance, angle, covariance );
00762   }
00763   m_robot_poses.unlock();
00764   m_opponents.unlock();
00765
00766   m_new_data_available = true;
00767 }
00768
00769 
00770 /** Remove the opponent with the given ID form the list of opponents
00771  * seen by the given robot.
00772  * @param host the hostname of the robot
00773  * @param uid the uid of the opponent
00774  */
00775 void
00776 WorldInfoDataContainer::opponent_disappeared( const char* host, unsigned int uid )
00777 {
00778   unsigned int id = get_host_id( host );
00779
00780   m_opponents.lock();
00781   OpponentsLockMap::iterator iter = m_opponents.find( id );
00782   if ( iter != m_opponents.end() )
00783     { iter->second.disappeared( uid ); }
00784   m_opponents.unlock();
00785
00786   m_new_data_available = true;
00787 }
00788
00789 
00790 /** Get all oppenents detected by a certain robot.
00791  * @param host hostname of the robot
00792  * @param opp_positions map containing the positions of the detected
00793  * opponents
00794  * @return false if no data about opponents is available from the
00795  * given robot
00796  */
00797 bool
00798 WorldInfoDataContainer::get_opponent_pos( const char* host,
00799                                           map<unsigned int, HomPoint>& opp_positions )
00800 {
00801   bool found = false;
00802   unsigned int id = get_host_id( host );
00803
00804   m_opponents.lock();
00805   OpponentsLockMap::iterator iter = m_opponents.find( id );
00806   if ( iter != m_opponents.end() )
00807   {
00808     opp_positions = iter->second.positions();
00809     found = true;
00810   }
00811   m_opponents.unlock();
00812
00813   return found;
00814 }
00815 
00816 /** Set the gamestate.
00817  * @param game_state the current game state
00818  * @param state_team team association of the game state
00819  * @param score_cyan score of the cyan-colored team
00820  * @param score_magenta score of the magenta-colored team
00821  * @param own_team own team color
00822  * @param own_goal_color own goal color
00823  * @param half first or second half
00824  */
00825 void
00826 WorldInfoDataContainer::set_game_state( int game_state,
00827                                         worldinfo_gamestate_team_t state_team,
00828                                         unsigned int score_cyan,
00829                                         unsigned int score_magenta,
00830                                         worldinfo_gamestate_team_t own_team,
00831                                         worldinfo_gamestate_goalcolor_t own_goal_color,
00832                                         worldinfo_gamestate_half_t half )
00833 {
00834   m_game_state.game_state    = game_state;
00835   m_game_state.state_team    = state_team;
00836   m_game_state.score_cyan    = score_cyan;
00837   m_game_state.score_magenta = score_magenta;
00838   m_game_state.half          = half;
00839
00840   m_own_team_color = own_team;
00841   m_own_goal_color = own_goal_color;
00842 }
00843 
00844 /** Obtain the game state.
00845  * @return the current game state
00846  */
00847 WorldInfoDataContainer::GameState
00848 WorldInfoDataContainer::get_game_state() const
00849 {
00850   return m_game_state;
00851 }
00852 
00853 /** Get the current game state as string.
00854  * @return the current game mode
00855  */
00856 std::string
00857 WorldInfoDataContainer::get_game_state_string() const
00858 {
00859   char* game_state;
00860   asprintf( &game_state, "%s [%s]",
00861             worldinfo_msl_gamestate_tostring((worldinfo_msl_gamestate_t)m_game_state.game_state),
00862             worldinfo_gamestate_team_tostring(m_game_state.state_team) );
00863
00864   string state_string(game_state);
00865   free(game_state);
00866   return state_string;
00867 }
00868 
00869 /** Get the current half as string.
00870  * @return the current half
00871  */
00872 std::string
00873 WorldInfoDataContainer::get_half_string() const
00874 {
00875   const char* half = worldinfo_gamestate_half_tostring(m_game_state.half);
00876
00877   return string(half);
00878 }
00879 
00880 /** Get own score.
00881  * @return own score
00882  */
00883 unsigned int
00884 WorldInfoDataContainer::get_own_score() const
00885 {
00886   if (m_own_team_color == TEAM_CYAN)
00887     { return m_game_state.score_cyan; }
00888   else
00889     { return m_game_state.score_magenta; }
00890 }
00891 
00892 /** Get score of the other team.
00893  * @return the other team's score
00894  */
00895 unsigned int
00896 WorldInfoDataContainer::get_other_score() const
00897 {
00898   if (m_own_team_color == TEAM_CYAN)
00899     { return m_game_state.score_magenta; }
00900   else
00901     { return m_game_state.score_cyan; }
00902 }
00903 
00904 /** Get own team color.
00905  * @return struct containing the own team color
00906  */
00907 worldinfo_gamestate_team_t
00908 WorldInfoDataContainer::get_own_team_color() const
00909 {
00910   return m_own_team_color;
00911 }
00912 
00913 /** Get own team color as string.
00914  * @return string with the own team color
00915  */
00916 std::string
00917 WorldInfoDataContainer::get_own_team_color_string() const
00918 {
00919   const char* team_color = worldinfo_gamestate_team_tostring(m_own_team_color);
00920
00921   return string(team_color);
00922 }
00923 
00924 /** Get own goal color.
00925  * @return struct containing the own goal color
00926  */
00927 worldinfo_gamestate_goalcolor_t
00928 WorldInfoDataContainer::get_own_goal_color() const
00929 {
00930   return m_own_goal_color;
00931 }
00932 
00933 /** Get own goal color as string.
00934  * @return string with the current goal color
00935  */
00936 std::string
00937 WorldInfoDataContainer::get_own_goal_color_string() const
00938 {
00939   const char* goal_color = worldinfo_gamestate_goalcolor_tostring(m_own_goal_color);
00940
00941   return string(goal_color);
00942 }
00943
00944 unsigned int
00945 WorldInfoDataContainer::get_host_id(std::string host)
00946 {
00947   unsigned int id;
00948
00949   m_hosts.lock();
00950   HostLockMap::iterator iter = m_hosts.find(host);
00951   if ( iter == m_hosts.end() )
00952     {
00953       id            = m_host_id++;
00954       m_hosts[host] = id;
00955       m_new_host    = true;
00956     }
00957   else
00958     { id = iter->second; }
00959   m_hosts.unlock();
00960
00961   return id;
00962 }
00963
00964 void
00965 WorldInfoDataContainer::clock_in_host(unsigned int id)
00966 {
00967   Time now(m_clock);
00968   now.stamp();
00969
00970   m_last_seen.lock();
00971   m_last_seen[id] = now.in_msec();
00972   m_last_seen.unlock();
00973 }
00974
00975 } // end namespace fawkes