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