23 #include <worldinfo_utils/data_container.h>
24 #include <utils/time/clock.h>
25 #include <utils/time/time.h>
26 #include <core/exceptions/system.h>
36 WorldInfoDataContainer::BallRecord::BallRecord()
41 WorldInfoDataContainer::BallRecord::~BallRecord()
46 WorldInfoDataContainer::BallRecord::set_pos(
float dist,
52 m_rel_pos.phi(bearing);
57 WorldInfoDataContainer::BallRecord::set_pos_global(
float x,
70 WorldInfoDataContainer::BallRecord::set_visible(
bool visible,
71 int visibility_history )
74 m_visibility_history = visibility_history;
78 WorldInfoDataContainer::BallRecord::set_velocity(
float vel_x,
90 WorldInfoDataContainer::BallRecord::visible()
const
96 WorldInfoDataContainer::BallRecord::visibility_history()
const
98 return m_visibility_history;
102 WorldInfoDataContainer::BallRecord::pos_relative()
108 WorldInfoDataContainer::BallRecord::vel_relative()
114 WorldInfoDataContainer::BallRecord::covariance_relative()
120 WorldInfoDataContainer::BallRecord::pos_global(
float ref_x,
126 HomPoint p( m_rel_pos.x(), m_rel_pos.y() );
127 p.rotate_z( ref_theta );
139 WorldInfoDataContainer::BallRecord::vel_global(
float vel_x,
145 return HomVector(0.0, 0.0, 0.0);
148 WorldInfoDataContainer::PoseRecord::PoseRecord()
152 WorldInfoDataContainer::PoseRecord::~PoseRecord()
157 WorldInfoDataContainer::PoseRecord::set_pose(
float x,
169 WorldInfoDataContainer::PoseRecord::set_velocity(
float vel_x,
174 m_velocity.x() = vel_x;
175 m_velocity.y() = vel_y;
176 m_velocity.z() = vel_theta;
181 WorldInfoDataContainer::PoseRecord::pose()
187 WorldInfoDataContainer::PoseRecord::pose_covariance()
189 return m_pose_covariance;
193 WorldInfoDataContainer::PoseRecord::velocity()
198 WorldInfoDataContainer::OpponentsRecord::OpponentsRecord()
202 WorldInfoDataContainer::OpponentsRecord::~OpponentsRecord()
207 WorldInfoDataContainer::OpponentsRecord::set_pos(
unsigned int id,
216 WorldInfoDataContainer::OpponentsRecord::set_pos( HomPose2d robot_pose,
220 float* rel_covariance )
222 HomTransform local_to_global;
223 local_to_global.rotate_z( robot_pose.yaw() );
224 local_to_global.trans( robot_pose.x(), robot_pose.y() );
225 HomPoint o = local_to_global * HomPoint( cos( rel_bearing ) * rel_distance,
226 sin( rel_bearing ) * rel_distance );
228 m_glob_opp_positions[ opp_id ] = o;
234 WorldInfoDataContainer::OpponentsRecord::disappeared(
unsigned int opp_id )
236 m_glob_opp_positions.erase( opp_id );
239 map<unsigned int, HomPoint>
240 WorldInfoDataContainer::OpponentsRecord::positions()
242 return m_glob_opp_positions;
256 WorldInfoDataContainer::WorldInfoDataContainer(
Clock* clock,
260 m_timeout_msec = timeout_msec;
269 m_game_state.score_cyan = 0;
270 m_game_state.score_magenta = 0;
273 m_new_data_available =
false;
275 m_host_timedout =
false;
279 WorldInfoDataContainer::~WorldInfoDataContainer()
290 WorldInfoDataContainer::check_timeout()
295 m_timedout_hosts.lock();
296 m_timedout_hosts.clear();
297 m_timedout_hosts.unlock();
300 HostLockMap::iterator iter = m_hosts.begin();
301 while ( iter != m_hosts.end() )
303 unsigned int id = iter->second;
305 if ( now.
in_msec() - m_last_seen[id] < m_timeout_msec )
310 m_last_seen.erase(
id);
311 m_last_seen.unlock();
313 m_ball_positions.lock();
314 m_ball_positions.erase(
id);
315 m_ball_positions.unlock();
317 m_robot_poses.lock();
318 m_robot_poses.erase(
id);
319 m_robot_poses.unlock();
321 m_timedout_hosts.lock();
322 m_timedout_hosts.push_back(iter->first);
323 m_timedout_hosts.unlock();
325 m_hosts.erase(iter++);
326 m_host_timedout =
true;
332 return m_timedout_hosts.size() != 0;
339 WorldInfoDataContainer::set_timeout(
long msec)
341 m_timeout_msec = msec;
349 std::list<std::string>
350 WorldInfoDataContainer::get_hosts(
bool check_timeout_first)
352 if (check_timeout_first)
358 for ( HostLockMap::iterator iter = m_hosts.begin();
359 iter != m_hosts.end();
361 { hosts.push_back( iter->first ); }
372 std::list<std::string>
373 WorldInfoDataContainer::get_timedout_hosts()
375 list<string> timedout_hosts;
377 m_timedout_hosts.lock();
378 for ( HostLockList::iterator iter = m_timedout_hosts.begin();
379 iter != m_timedout_hosts.end();
381 { timedout_hosts.push_back( *iter ); }
382 m_timedout_hosts.unlock();
384 return timedout_hosts;
391 WorldInfoDataContainer::new_data_available()
393 bool new_data = m_new_data_available;
394 m_new_data_available =
false;
402 WorldInfoDataContainer::new_host()
404 bool new_host = m_new_host;
413 WorldInfoDataContainer::host_timedout()
415 bool host_timedout = m_host_timedout;
416 m_host_timedout =
false;
417 return host_timedout;
430 WorldInfoDataContainer::set_robot_pose(
const char* host,
436 PoseLockMap::iterator iter;
437 unsigned int id = get_host_id( host );
440 m_robot_poses.lock();
441 iter = m_robot_poses.find(
id );
442 if ( iter == m_robot_poses.end() )
444 PoseRecord pose_record;
445 pose_record.set_pose( x, y, theta, covariance );
446 m_robot_poses[ id ] = pose_record;
450 iter->second.set_pose( x, y, theta, covariance );
452 m_robot_poses.unlock();
454 m_new_data_available =
true;
463 WorldInfoDataContainer::get_robot_pose(
const char* host,
467 unsigned int id = get_host_id( host );
469 m_robot_poses.lock();
470 PoseLockMap::iterator iter = m_robot_poses.find(
id );
472 if ( iter != m_robot_poses.end() )
474 pose = iter->second.pose();
477 m_robot_poses.unlock();
492 WorldInfoDataContainer::get_robot_pose(
const char* host,
497 unsigned int id = get_host_id( host );
499 m_robot_poses.lock();
500 PoseLockMap::iterator iter = m_robot_poses.find(
id );
502 if ( iter != m_robot_poses.end() )
504 pose = iter->second.pose();
505 pose_cov = iter->second.pose_covariance();
508 m_robot_poses.unlock();
522 WorldInfoDataContainer::set_robot_velocity(
const char* host,
528 PoseLockMap::iterator iter;
529 unsigned int id = get_host_id( host );
532 m_robot_poses.lock();
533 iter = m_robot_poses.find(
id );
534 if ( iter == m_robot_poses.end() )
536 PoseRecord pose_record;
537 pose_record.set_velocity( vel_x, vel_y, vel_theta, covariance );
538 m_robot_poses[ id ] = pose_record;
542 iter->second.set_velocity( vel_x, vel_y, vel_theta, covariance );
544 m_robot_poses.unlock();
546 m_new_data_available =
true;
558 WorldInfoDataContainer::get_robot_velocity(
const char* host,
576 WorldInfoDataContainer::set_ball_pos(
const char* host,
578 int visibility_history,
584 BallLockMap::iterator iter;
585 unsigned int id = get_host_id( host );
588 m_ball_positions.lock();
589 iter = m_ball_positions.find(
id );
590 if ( iter == m_ball_positions.end() )
592 BallRecord ball_record;
593 ball_record.set_visible( visible, visibility_history );
594 ball_record.set_pos( dist, bearing, slope, covariance );
595 m_ball_positions[ id ] = ball_record;
599 iter->second.set_visible( visible, visibility_history );
600 iter->second.set_pos( dist, bearing, slope, covariance );
602 m_ball_positions.unlock();
604 m_new_data_available =
true;
617 WorldInfoDataContainer::set_ball_pos_global(
const char* host,
619 int visibility_history,
625 BallLockMap::iterator iter;
626 unsigned int id = get_host_id( host );
629 m_ball_positions.lock();
630 iter = m_ball_positions.find(
id );
631 if ( iter == m_ball_positions.end() )
633 BallRecord ball_record;
634 ball_record.set_visible( visible, visibility_history );
635 ball_record.set_pos_global( x, y, z, covariance );
636 m_ball_positions[ id ] = ball_record;
640 iter->second.set_visible( visible, visibility_history );
641 iter->second.set_pos_global( x, y, z, covariance );
643 m_ball_positions.unlock();
645 m_new_data_available =
true;
655 WorldInfoDataContainer::get_ball_pos_relative(
const char* host,
659 unsigned int id = get_host_id( host );
661 m_ball_positions.lock();
662 BallLockMap::iterator iter = m_ball_positions.find(
id );
664 if ( iter != m_ball_positions.end() )
666 pos = iter->second.pos_relative();
667 found = iter->second.visible();
669 m_ball_positions.unlock();
683 WorldInfoDataContainer::get_ball_pos_relative(
const char* host,
688 unsigned int id = get_host_id( host );
690 m_ball_positions.lock();
691 BallLockMap::iterator iter = m_ball_positions.find(
id );
693 if ( iter != m_ball_positions.end() )
695 pos = iter->second.pos_relative();
696 pos_cov = iter->second.covariance_relative();
697 found = iter->second.visible();
699 m_ball_positions.unlock();
713 WorldInfoDataContainer::get_ball_pos_global(
const char* host,
717 unsigned int id = get_host_id( host );
719 m_ball_positions.lock();
720 m_robot_poses.lock();
721 BallLockMap::iterator ball_iter = m_ball_positions.find(
id );
722 PoseLockMap::iterator pose_iter = m_robot_poses.find(
id );
724 if ( ball_iter != m_ball_positions.end() &&
725 pose_iter != m_robot_poses.end() )
727 HomPose2d robot_pose = pose_iter->second.pose();
728 pos = ball_iter->second.pos_global( robot_pose.
x(),
731 found = ball_iter->second.visible();
733 m_robot_poses.unlock();
734 m_ball_positions.unlock();
751 WorldInfoDataContainer::set_ball_velocity(
const char* host,
757 BallLockMap::iterator iter;
758 unsigned int id = get_host_id( host );
761 m_ball_positions.lock();
762 iter = m_ball_positions.find(
id );
763 if ( iter == m_ball_positions.end() )
765 BallRecord ball_record;
766 ball_record.set_velocity( vel_x, vel_y, vel_z, covariance );
767 m_ball_positions[ id ] = ball_record;
771 iter->second.set_velocity( vel_x, vel_y, vel_z, covariance );
773 m_ball_positions.unlock();
775 m_new_data_available =
true;
787 WorldInfoDataContainer::get_ball_velocity(
const char* host,
802 WorldInfoDataContainer::set_opponent_pos(
const char* host,
808 unsigned int id = get_host_id( host );
812 m_robot_poses.lock();
813 OpponentsLockMap::iterator oit = m_opponents.find(
id );
814 PoseLockMap::iterator pit = m_robot_poses.find(
id );
817 if ( pit != m_robot_poses.end() )
818 { pose = pit->second.pose(); }
820 if ( oit == m_opponents.end() )
822 OpponentsRecord opponents_record;
823 opponents_record.set_pos( pose, uid, distance, angle, covariance );
824 m_opponents[ id ] = opponents_record;
828 oit->second.set_pos( pose, uid, distance, angle, covariance );
830 m_robot_poses.unlock();
831 m_opponents.unlock();
833 m_new_data_available =
true;
843 WorldInfoDataContainer::opponent_disappeared(
const char* host,
unsigned int uid )
845 unsigned int id = get_host_id( host );
848 OpponentsLockMap::iterator iter = m_opponents.find(
id );
849 if ( iter != m_opponents.end() )
850 { iter->second.disappeared( uid ); }
851 m_opponents.unlock();
853 m_new_data_available =
true;
865 WorldInfoDataContainer::get_opponent_pos(
const char* host,
866 map<unsigned int, HomPoint>& opp_positions )
869 unsigned int id = get_host_id( host );
872 OpponentsLockMap::iterator iter = m_opponents.find(
id );
873 if ( iter != m_opponents.end() )
875 opp_positions = iter->second.positions();
878 m_opponents.unlock();
893 WorldInfoDataContainer::set_game_state(
int game_state,
895 unsigned int score_cyan,
896 unsigned int score_magenta,
901 m_game_state.game_state = game_state;
902 m_game_state.state_team = state_team;
903 m_game_state.score_cyan = score_cyan;
904 m_game_state.score_magenta = score_magenta;
905 m_game_state.half = half;
907 m_own_team_color = own_team;
908 m_own_goal_color = own_goal_color;
915 WorldInfoDataContainer::get_game_state()
const
924 WorldInfoDataContainer::get_game_state_string()
const
927 if (asprintf( &game_state,
"%s [%s]",
933 string state_string(game_state);
942 WorldInfoDataContainer::get_half_string()
const
953 WorldInfoDataContainer::get_own_score()
const
956 {
return m_game_state.score_cyan; }
958 {
return m_game_state.score_magenta; }
965 WorldInfoDataContainer::get_other_score()
const
968 {
return m_game_state.score_magenta; }
970 {
return m_game_state.score_cyan; }
977 WorldInfoDataContainer::get_own_team_color()
const
979 return m_own_team_color;
986 WorldInfoDataContainer::get_own_team_color_string()
const
990 return string(team_color);
997 WorldInfoDataContainer::get_own_goal_color()
const
999 return m_own_goal_color;
1006 WorldInfoDataContainer::get_own_goal_color_string()
const
1010 return string(goal_color);
1014 WorldInfoDataContainer::get_host_id(std::string host)
1019 HostLockMap::iterator iter = m_hosts.find(host);
1020 if ( iter == m_hosts.end() )
1027 {
id = iter->second; }
1034 WorldInfoDataContainer::clock_in_host(
unsigned int id)
1040 m_last_seen[id] = now.in_msec();
1041 m_last_seen.unlock();