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