Fawkes API
Fawkes Development Version
|
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