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 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