Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
data_container.cpp
1 
2 /***************************************************************************
3  * data_container.cpp - World info data container
4  *
5  * Created: Thu April 10 22:23:27 2008
6  * Copyright 2008 Daniel Beck
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
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>
27 #include <cstdlib>
28 #include <cstdio>
29 #include <cmath>
30 
31 using namespace std;
32 namespace fawkes {
33 
34 /// @cond INTERNALS
35 
36 WorldInfoDataContainer::BallRecord::BallRecord()
37 {
38  m_is_global = false;
39 }
40 
41 WorldInfoDataContainer::BallRecord::~BallRecord()
42 {
43 }
44 
45 void
46 WorldInfoDataContainer::BallRecord::set_pos( float dist,
47  float bearing,
48  float slope,
49  float* covariance )
50 {
51  m_rel_pos.r(dist);
52  m_rel_pos.phi(bearing);
53  // TODO: slope, covariance
54 }
55 
56 void
57 WorldInfoDataContainer::BallRecord::set_pos_global( float x,
58  float y,
59  float z,
60  float* covariance )
61 {
62  m_is_global = true;
63  m_glob_pos.x( x );
64  m_glob_pos.y( y );
65  m_glob_pos.z( z );
66  // TODO: covarince
67 }
68 
69 void
70 WorldInfoDataContainer::BallRecord::set_visible( bool visible,
71  int visibility_history )
72 {
73  m_visible = visible;
74  m_visibility_history = visibility_history;
75 }
76 
77 void
78 WorldInfoDataContainer::BallRecord::set_velocity( float vel_x,
79  float vel_y,
80  float vel_z,
81  float* covariance )
82 {
83  m_rel_vel.x(vel_x);
84  m_rel_vel.y(vel_y);
85  m_rel_vel.z(vel_z);
86  // TODO: covariance
87 }
88 
89 bool
90 WorldInfoDataContainer::BallRecord::visible() const
91 {
92  return m_visible;
93 }
94 
95 int
96 WorldInfoDataContainer::BallRecord::visibility_history() const
97 {
98  return m_visibility_history;
99 }
100 
101 HomPolar
102 WorldInfoDataContainer::BallRecord::pos_relative()
103 {
104  return m_rel_pos;
105 }
106 
107 HomVector
108 WorldInfoDataContainer::BallRecord::vel_relative()
109 {
110  return m_rel_vel;
111 }
112 
113 Matrix
114 WorldInfoDataContainer::BallRecord::covariance_relative()
115 {
116  return m_rel_cov;
117 }
118 
119 HomPoint
120 WorldInfoDataContainer::BallRecord::pos_global( float ref_x,
121  float ref_y,
122  float ref_theta )
123 {
124  if ( !m_is_global )
125  {
126  HomPoint p( m_rel_pos.x(), m_rel_pos.y() );
127  p.rotate_z( ref_theta );
128  p.x() += ref_x;
129  p.y() += ref_y;
130  return p;
131  }
132  else
133  {
134  return m_glob_pos;
135  }
136 }
137 
138 HomVector
139 WorldInfoDataContainer::BallRecord::vel_global( float vel_x,
140  float vel_y,
141  float vel_theta,
142  float ref_theta )
143 {
144  // TODO
145  return HomVector(0.0, 0.0, 0.0);
146 }
147 
148 WorldInfoDataContainer::PoseRecord::PoseRecord()
149 {
150 }
151 
152 WorldInfoDataContainer::PoseRecord::~PoseRecord()
153 {
154 }
155 
156 void
157 WorldInfoDataContainer::PoseRecord::set_pose( float x,
158  float y,
159  float theta,
160  float* covariance )
161 {
162  m_pose.x( x );
163  m_pose.y( y );
164  m_pose.yaw( theta );
165  // TODO: covariance
166 }
167 
168 void
169 WorldInfoDataContainer::PoseRecord::set_velocity( float vel_x,
170  float vel_y,
171  float vel_theta,
172  float* covariance )
173 {
174  m_velocity.x() = vel_x;
175  m_velocity.y() = vel_y;
176  m_velocity.z() = vel_theta;
177  // TODO: covariance
178 }
179 
180 HomPose2d
181 WorldInfoDataContainer::PoseRecord::pose()
182 {
183  return m_pose;
184 }
185 
186 Matrix
187 WorldInfoDataContainer::PoseRecord::pose_covariance()
188 {
189  return m_pose_covariance;
190 }
191 
192 HomVector
193 WorldInfoDataContainer::PoseRecord::velocity()
194 {
195  return m_velocity;
196 }
197 
198 WorldInfoDataContainer::OpponentsRecord::OpponentsRecord()
199 {
200 }
201 
202 WorldInfoDataContainer::OpponentsRecord::~OpponentsRecord()
203 {
204 }
205 
206 void
207 WorldInfoDataContainer::OpponentsRecord::set_pos( unsigned int id,
208  float distance,
209  float bearing,
210  float* covariance )
211 {
212  // TODO
213 }
214 
215 void
216 WorldInfoDataContainer::OpponentsRecord::set_pos( HomPose2d robot_pose,
217  unsigned int opp_id,
218  float rel_distance,
219  float rel_bearing,
220  float* rel_covariance )
221 {
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 );
227 
228  m_glob_opp_positions[ opp_id ] = o;
229 
230  // TODO: covariance
231 }
232 
233 void
234 WorldInfoDataContainer::OpponentsRecord::disappeared( unsigned int opp_id )
235 {
236  m_glob_opp_positions.erase( opp_id );
237 }
238 
239 map<unsigned int, HomPoint>
240 WorldInfoDataContainer::OpponentsRecord::positions()
241 {
242  return m_glob_opp_positions;
243 }
244 
245 /// @endcond
246 
247 /** @class WorldInfoDataContainer <worldinfo_utils/data_container.h>
248  * Data container to store and exchange worldinfo data.
249  * @author Daniel Beck
250  */
251 
252 /** Constructor.
253  * @param clock pointer to a Clock
254  * @param timeout_msec timeout in milliseconds
255  */
256 WorldInfoDataContainer::WorldInfoDataContainer( Clock* clock,
257  long timeout_msec )
258 {
259  m_clock = clock;
260  m_timeout_msec = timeout_msec;
261 
262  m_host_id = 0;
263 
264  m_own_team_color = TEAM_CYAN;
265  m_own_goal_color = GOAL_BLUE;
266 
267  m_game_state.game_state = GS_FROZEN;
268  m_game_state.state_team = TEAM_BOTH;
269  m_game_state.score_cyan = 0;
270  m_game_state.score_magenta = 0;
271  m_game_state.half = HALF_FIRST;
272 
273  m_new_data_available = false;
274  m_new_host = false;
275  m_host_timedout = false;
276 }
277 
278 /** Destructor. */
279 WorldInfoDataContainer::~WorldInfoDataContainer()
280 {
281 }
282 
283 /** Check for timed out hosts.
284  * This method should be called regularly to remove hosts from the
285  * data container from which no data has been received in a certain
286  * amount of time.
287  * @return true if there are timed out hosts
288  */
289 bool
290 WorldInfoDataContainer::check_timeout()
291 {
292  Time now(m_clock);
293  now.stamp();
294 
295  m_timedout_hosts.lock();
296  m_timedout_hosts.clear();
297  m_timedout_hosts.unlock();
298 
299  m_hosts.lock();
300  HostLockMap::iterator iter = m_hosts.begin();
301  while ( iter != m_hosts.end() )
302  {
303  unsigned int id = iter->second;
304 
305  if ( now.in_msec() - m_last_seen[id] < m_timeout_msec )
306  { ++iter; }
307  else
308  {
309  m_last_seen.lock();
310  m_last_seen.erase(id);
311  m_last_seen.unlock();
312 
313  m_ball_positions.lock();
314  m_ball_positions.erase(id);
315  m_ball_positions.unlock();
316 
317  m_robot_poses.lock();
318  m_robot_poses.erase(id);
319  m_robot_poses.unlock();
320 
321  m_timedout_hosts.lock();
322  m_timedout_hosts.push_back(iter->first);
323  m_timedout_hosts.unlock();
324 
325  m_hosts.erase(iter++);
326  m_host_timedout = true;
327  }
328  }
329 
330  m_hosts.unlock();
331 
332  return m_timedout_hosts.size() != 0;
333 }
334 
335 /** Set the time out.
336  * @param msec time out value in milliseconds
337  */
338 void
339 WorldInfoDataContainer::set_timeout(long msec)
340 {
341  m_timeout_msec = msec;
342 }
343 
344 /** Obtain the list of active hosts.
345  * @param check_timeout_first if true check_timeout() is called before
346  * the list is compiled
347  * @return the list of active hosts
348  */
349 std::list<std::string>
350 WorldInfoDataContainer::get_hosts(bool check_timeout_first)
351 {
352  if (check_timeout_first)
353  { check_timeout(); }
354 
355  list<string> hosts;
356 
357  m_hosts.lock();
358  for ( HostLockMap::iterator iter = m_hosts.begin();
359  iter != m_hosts.end();
360  ++iter )
361  { hosts.push_back( iter->first ); }
362  m_hosts.unlock();
363 
364  return hosts;
365 }
366 
367 /** Obtain the list of timedout hosts.
368  * Hosts that have been marked as timedout in the last call of
369  * check_timeout().
370  * @return the list of timedout hosts
371  */
372 std::list<std::string>
373 WorldInfoDataContainer::get_timedout_hosts()
374 {
375  list<string> timedout_hosts;
376 
377  m_timedout_hosts.lock();
378  for ( HostLockList::iterator iter = m_timedout_hosts.begin();
379  iter != m_timedout_hosts.end();
380  ++iter )
381  { timedout_hosts.push_back( *iter ); }
382  m_timedout_hosts.unlock();
383 
384  return timedout_hosts;
385 }
386 
387 /** Check whehter new data is available.
388  * @return true if new data is available.
389  */
390 bool
391 WorldInfoDataContainer::new_data_available()
392 {
393  bool new_data = m_new_data_available;
394  m_new_data_available = false;
395  return new_data;
396 }
397 
398 /** Check whether a new host has been added recently.
399  * @return true if a new host has been added recently
400  */
401 bool
402 WorldInfoDataContainer::new_host()
403 {
404  bool new_host = m_new_host;
405  m_new_host = false;
406  return new_host;
407 }
408 
409 /** Check whether a host has timed out.
410  * @return true if a host has timed out recently
411  */
412 bool
413 WorldInfoDataContainer::host_timedout()
414 {
415  bool host_timedout = m_host_timedout;
416  m_host_timedout = false;
417  return host_timedout;
418 }
419 
420 
421 /** Set the pose of a robot.
422  * @param host the hostname of the robot
423  * @param x the x-coordinate of the robot's global position
424  * @param y the y-coordinate of the robot's global position
425  * @param theta the global orientation of the robot
426  * @param covariance covariance associated with the position
427  * estimation
428  */
429 void
430 WorldInfoDataContainer::set_robot_pose( const char* host,
431  float x,
432  float y,
433  float theta,
434  float* covariance )
435 {
436  PoseLockMap::iterator iter;
437  unsigned int id = get_host_id( host );
438  clock_in_host( id );
439 
440  m_robot_poses.lock();
441  iter = m_robot_poses.find( id );
442  if ( iter == m_robot_poses.end() )
443  {
444  PoseRecord pose_record;
445  pose_record.set_pose( x, y, theta, covariance );
446  m_robot_poses[ id ] = pose_record;
447  }
448  else
449  {
450  iter->second.set_pose( x, y, theta, covariance );
451  }
452  m_robot_poses.unlock();
453 
454  m_new_data_available = true;
455 }
456 
457 /** Obtain the pose of the given robot.
458  * @param host the hostname of the robot
459  * @param pose reference to a HomPose where the pose will be stored
460  * @return false if no pose for the requested robot could be found
461  */
462 bool
463 WorldInfoDataContainer::get_robot_pose( const char* host,
464  HomPose2d& pose )
465 {
466  bool found = false;
467  unsigned int id = get_host_id( host );
468 
469  m_robot_poses.lock();
470  PoseLockMap::iterator iter = m_robot_poses.find( id );
471 
472  if ( iter != m_robot_poses.end() )
473  {
474  pose = iter->second.pose();
475  found = true;
476  }
477  m_robot_poses.unlock();
478 
479  return found;
480 }
481 
482 
483 /** Get the position of a certain robot.
484  * @param host the hostname of the robot
485  * @param pose reference to a HomPoint where the global position of
486  * the robot is written to
487  * @param pose_cov reference to a Matrix where the covariance of the
488  * robot position is written to
489  * @return true if a pose for the robot could be found
490  */
491 bool
492 WorldInfoDataContainer::get_robot_pose( const char* host,
493  HomPose2d& pose,
494  Matrix& pose_cov )
495 {
496  bool found = false;
497  unsigned int id = get_host_id( host );
498 
499  m_robot_poses.lock();
500  PoseLockMap::iterator iter = m_robot_poses.find( id );
501 
502  if ( iter != m_robot_poses.end() )
503  {
504  pose = iter->second.pose();
505  pose_cov = iter->second.pose_covariance();
506  found = true;
507  }
508  m_robot_poses.unlock();
509 
510  return found;
511 }
512 
513 
514 /** Set the velocity of the robot.
515  * @param host the hostname of the robot
516  * @param vel_x the current forward velocity of the robot
517  * @param vel_y the current sideward velocity of the robot
518  * @param vel_theta the current rotational velociy of the robot
519  * @param covariance the velocity covariance
520  */
521 void
522 WorldInfoDataContainer::set_robot_velocity( const char* host,
523  float vel_x,
524  float vel_y,
525  float vel_theta,
526  float* covariance )
527 {
528  PoseLockMap::iterator iter;
529  unsigned int id = get_host_id( host );
530  clock_in_host( id );
531 
532  m_robot_poses.lock();
533  iter = m_robot_poses.find( id );
534  if ( iter == m_robot_poses.end() )
535  {
536  PoseRecord pose_record;
537  pose_record.set_velocity( vel_x, vel_y, vel_theta, covariance );
538  m_robot_poses[ id ] = pose_record;
539  }
540  else
541  {
542  iter->second.set_velocity( vel_x, vel_y, vel_theta, covariance );
543  }
544  m_robot_poses.unlock();
545 
546  m_new_data_available = true;
547 }
548 
549 
550 /** Obtain current velocity of the specified robot.
551  * @param host the hostname of the robot
552  * @param robot_vel reference to a HomVector where the velocity
553  * information is written to
554  * @return true, if velocity information for the specified host are
555  * available
556  */
557 bool
558 WorldInfoDataContainer::get_robot_velocity( const char* host,
559  HomVector& robot_vel )
560 {
561  // TODO
562  return true;
563 }
564 
565 
566 /** Set the ball position estimation of a robot.
567  * @param host the hostname of the robot
568  * @param visible visible or not
569  * @param visibility_history visible/not visible for n iterations
570  * @param dist distance to the robot
571  * @param bearing vertical angle to the ball
572  * @param slope the horizontal angle to the ball
573  * @param covariance covariance associated with the position estimation
574  */
575 void
576 WorldInfoDataContainer::set_ball_pos( const char* host,
577  bool visible,
578  int visibility_history,
579  float dist,
580  float bearing,
581  float slope,
582  float* covariance )
583 {
584  BallLockMap::iterator iter;
585  unsigned int id = get_host_id( host );
586  clock_in_host( id );
587 
588  m_ball_positions.lock();
589  iter = m_ball_positions.find( id );
590  if ( iter == m_ball_positions.end() )
591  {
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;
596  }
597  else
598  {
599  iter->second.set_visible( visible, visibility_history );
600  iter->second.set_pos( dist, bearing, slope, covariance );
601  }
602  m_ball_positions.unlock();
603 
604  m_new_data_available = true;
605 }
606 
607 /** Set the global ball position estimation of a robot.
608  * @param host the hostname of the robot
609  * @param visible visible or not
610  * @param visibility_history visible/not visible for n iterations
611  * @param x the x-coordinte of the global ball position
612  * @param y the y-coordinte of the global ball position
613  * @param z the z-coordinte of the global ball position
614  * @param covariance covariance associated with the position estimation
615  */
616 void
617 WorldInfoDataContainer::set_ball_pos_global( const char* host,
618  bool visible,
619  int visibility_history,
620  float x,
621  float y,
622  float z,
623  float* covariance )
624 {
625  BallLockMap::iterator iter;
626  unsigned int id = get_host_id( host );
627  clock_in_host( id );
628 
629  m_ball_positions.lock();
630  iter = m_ball_positions.find( id );
631  if ( iter == m_ball_positions.end() )
632  {
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;
637  }
638  else
639  {
640  iter->second.set_visible( visible, visibility_history );
641  iter->second.set_pos_global( x, y, z, covariance );
642  }
643  m_ball_positions.unlock();
644 
645  m_new_data_available = true;
646 }
647 
648 
649 /** Get the ball position estimation of a certain robot.
650  * @param host the hostname of the robot
651  * @param pos reference to a HomPolar where the position is written to
652  * @return true if a global ball position was found
653  */
654 bool
655 WorldInfoDataContainer::get_ball_pos_relative( const char* host,
656  HomPolar& pos )
657 {
658  bool found = false;
659  unsigned int id = get_host_id( host );
660 
661  m_ball_positions.lock();
662  BallLockMap::iterator iter = m_ball_positions.find( id );
663 
664  if ( iter != m_ball_positions.end() )
665  {
666  pos = iter->second.pos_relative();
667  found = iter->second.visible();
668  }
669  m_ball_positions.unlock();
670 
671  return found;
672 }
673 
674 
675 /** Get the ball position estimation of a certain robot.
676  * @param host the hostname of the robot
677  * @param pos reference to a HomPolar where the position is written to
678  * @param pos_cov reference to a Matrix where the ball position
679  * covariance is written to
680  * @return true if a global ball position was found
681  */
682 bool
683 WorldInfoDataContainer::get_ball_pos_relative( const char* host,
684  HomPolar& pos,
685  Matrix& pos_cov )
686 {
687  bool found = false;
688  unsigned int id = get_host_id( host );
689 
690  m_ball_positions.lock();
691  BallLockMap::iterator iter = m_ball_positions.find( id );
692 
693  if ( iter != m_ball_positions.end() )
694  {
695  pos = iter->second.pos_relative();
696  pos_cov = iter->second.covariance_relative();
697  found = iter->second.visible();
698  }
699  m_ball_positions.unlock();
700 
701  return found;
702 }
703 
704 
705 /** Get the global position of the ball as it is estimated by the
706  * specified robot.
707  * @param host the robot's hostname
708  * @param pos refercence to a HomPoint where the position of the ball
709  * written to
710  * @return true if position was found/received, false otherwise
711  */
712 bool
713 WorldInfoDataContainer::get_ball_pos_global( const char* host,
714  HomPoint& pos )
715 {
716  bool found = false;
717  unsigned int id = get_host_id( host );
718 
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 );
723 
724  if ( ball_iter != m_ball_positions.end() &&
725  pose_iter != m_robot_poses.end() )
726  {
727  HomPose2d robot_pose = pose_iter->second.pose();
728  pos = ball_iter->second.pos_global( robot_pose.x(),
729  robot_pose.y(),
730  robot_pose.yaw() );
731  found = ball_iter->second.visible();
732  }
733  m_robot_poses.unlock();
734  m_ball_positions.unlock();
735 
736  return found;
737 }
738 
739 
740 /** Set the ball velocity as it is estimated by the specified robot.
741  * @param host the hostname of the robot
742  * @param vel_x the ball velocity in x-direction of the robot-centered
743  * coordinate system
744  * @param vel_y the ball velocity in y-direction of the robot-centered
745  * coordinate system
746  * @param vel_z the ball velocity in z-direction of the robot-centered
747  * coordinate system
748  * @param covariance ball velocity covariance
749  */
750 void
751 WorldInfoDataContainer::set_ball_velocity( const char* host,
752  float vel_x,
753  float vel_y,
754  float vel_z,
755  float* covariance )
756 {
757  BallLockMap::iterator iter;
758  unsigned int id = get_host_id( host );
759  clock_in_host( id );
760 
761  m_ball_positions.lock();
762  iter = m_ball_positions.find( id );
763  if ( iter == m_ball_positions.end() )
764  {
765  BallRecord ball_record;
766  ball_record.set_velocity( vel_x, vel_y, vel_z, covariance );
767  m_ball_positions[ id ] = ball_record;
768  }
769  else
770  {
771  iter->second.set_velocity( vel_x, vel_y, vel_z, covariance );
772  }
773  m_ball_positions.unlock();
774 
775  m_new_data_available = true;
776 }
777 
778 
779 /** Obtain ball velocity information for specified robot.
780  * @param host the hostname of the robot
781  * @param ball_vel refrence to a HomVector where the velocity
782  * information is written to
783  * @return true if ball velocity information from the specified robot
784  * are available
785  */
786 bool
787 WorldInfoDataContainer::get_ball_velocity( const char* host,
788  HomVector& ball_vel )
789 {
790  // TODO
791  return true;
792 }
793 
794 /** Set the position of a detected opponent.
795  * @param host hostname of the robot that detected the robot
796  * @param uid opponent id
797  * @param distance distance to the robot
798  * @param angle angle at which the opponent is detected
799  * @param covariance corresponding covariance matrix
800  */
801 void
802 WorldInfoDataContainer::set_opponent_pos( const char* host,
803  unsigned int uid,
804  float distance,
805  float angle,
806  float* covariance )
807 {
808  unsigned int id = get_host_id( host );
809  clock_in_host( id );
810 
811  m_opponents.lock();
812  m_robot_poses.lock();
813  OpponentsLockMap::iterator oit = m_opponents.find( id );
814  PoseLockMap::iterator pit = m_robot_poses.find( id );
815 
816  HomPose2d pose;
817  if ( pit != m_robot_poses.end() )
818  { pose = pit->second.pose(); }
819 
820  if ( oit == m_opponents.end() )
821  {
822  OpponentsRecord opponents_record;
823  opponents_record.set_pos( pose, uid, distance, angle, covariance );
824  m_opponents[ id ] = opponents_record;
825  }
826  else
827  {
828  oit->second.set_pos( pose, uid, distance, angle, covariance );
829  }
830  m_robot_poses.unlock();
831  m_opponents.unlock();
832 
833  m_new_data_available = true;
834 }
835 
836 
837 /** Remove the opponent with the given ID form the list of opponents
838  * seen by the given robot.
839  * @param host the hostname of the robot
840  * @param uid the uid of the opponent
841  */
842 void
843 WorldInfoDataContainer::opponent_disappeared( const char* host, unsigned int uid )
844 {
845  unsigned int id = get_host_id( host );
846 
847  m_opponents.lock();
848  OpponentsLockMap::iterator iter = m_opponents.find( id );
849  if ( iter != m_opponents.end() )
850  { iter->second.disappeared( uid ); }
851  m_opponents.unlock();
852 
853  m_new_data_available = true;
854 }
855 
856 
857 /** Get all oppenents detected by a certain robot.
858  * @param host hostname of the robot
859  * @param opp_positions map containing the positions of the detected
860  * opponents
861  * @return false if no data about opponents is available from the
862  * given robot
863  */
864 bool
865 WorldInfoDataContainer::get_opponent_pos( const char* host,
866  map<unsigned int, HomPoint>& opp_positions )
867 {
868  bool found = false;
869  unsigned int id = get_host_id( host );
870 
871  m_opponents.lock();
872  OpponentsLockMap::iterator iter = m_opponents.find( id );
873  if ( iter != m_opponents.end() )
874  {
875  opp_positions = iter->second.positions();
876  found = true;
877  }
878  m_opponents.unlock();
879 
880  return found;
881 }
882 
883 /** Set the gamestate.
884  * @param game_state the current game state
885  * @param state_team team association of the game state
886  * @param score_cyan score of the cyan-colored team
887  * @param score_magenta score of the magenta-colored team
888  * @param own_team own team color
889  * @param own_goal_color own goal color
890  * @param half first or second half
891  */
892 void
893 WorldInfoDataContainer::set_game_state( int game_state,
894  worldinfo_gamestate_team_t state_team,
895  unsigned int score_cyan,
896  unsigned int score_magenta,
898  worldinfo_gamestate_goalcolor_t own_goal_color,
900 {
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;
906 
907  m_own_team_color = own_team;
908  m_own_goal_color = own_goal_color;
909 }
910 
911 /** Obtain the game state.
912  * @return the current game state
913  */
915 WorldInfoDataContainer::get_game_state() const
916 {
917  return m_game_state;
918 }
919 
920 /** Get the current game state as string.
921  * @return the current game mode
922  */
923 std::string
924 WorldInfoDataContainer::get_game_state_string() const
925 {
926  char* game_state;
927  if (asprintf( &game_state, "%s [%s]",
929  worldinfo_gamestate_team_tostring(m_game_state.state_team) ) == -1) {
930  throw OutOfMemoryException("Failed to allocate game state string");
931  }
932 
933  string state_string(game_state);
934  free(game_state);
935  return state_string;
936 }
937 
938 /** Get the current half as string.
939  * @return the current half
940  */
941 std::string
942 WorldInfoDataContainer::get_half_string() const
943 {
944  const char* half = worldinfo_gamestate_half_tostring(m_game_state.half);
945 
946  return string(half);
947 }
948 
949 /** Get own score.
950  * @return own score
951  */
952 unsigned int
953 WorldInfoDataContainer::get_own_score() const
954 {
955  if (m_own_team_color == TEAM_CYAN)
956  { return m_game_state.score_cyan; }
957  else
958  { return m_game_state.score_magenta; }
959 }
960 
961 /** Get score of the other team.
962  * @return the other team's score
963  */
964 unsigned int
965 WorldInfoDataContainer::get_other_score() const
966 {
967  if (m_own_team_color == TEAM_CYAN)
968  { return m_game_state.score_magenta; }
969  else
970  { return m_game_state.score_cyan; }
971 }
972 
973 /** Get own team color.
974  * @return struct containing the own team color
975  */
977 WorldInfoDataContainer::get_own_team_color() const
978 {
979  return m_own_team_color;
980 }
981 
982 /** Get own team color as string.
983  * @return string with the own team color
984  */
985 std::string
986 WorldInfoDataContainer::get_own_team_color_string() const
987 {
988  const char* team_color = worldinfo_gamestate_team_tostring(m_own_team_color);
989 
990  return string(team_color);
991 }
992 
993 /** Get own goal color.
994  * @return struct containing the own goal color
995  */
997 WorldInfoDataContainer::get_own_goal_color() const
998 {
999  return m_own_goal_color;
1000 }
1001 
1002 /** Get own goal color as string.
1003  * @return string with the current goal color
1004  */
1005 std::string
1006 WorldInfoDataContainer::get_own_goal_color_string() const
1007 {
1008  const char* goal_color = worldinfo_gamestate_goalcolor_tostring(m_own_goal_color);
1009 
1010  return string(goal_color);
1011 }
1012 
1013 unsigned int
1014 WorldInfoDataContainer::get_host_id(std::string host)
1015 {
1016  unsigned int id;
1017 
1018  m_hosts.lock();
1019  HostLockMap::iterator iter = m_hosts.find(host);
1020  if ( iter == m_hosts.end() )
1021  {
1022  id = m_host_id++;
1023  m_hosts[host] = id;
1024  m_new_host = true;
1025  }
1026  else
1027  { id = iter->second; }
1028  m_hosts.unlock();
1029 
1030  return id;
1031 }
1032 
1033 void
1034 WorldInfoDataContainer::clock_in_host(unsigned int id)
1035 {
1036  Time now(m_clock);
1037  now.stamp();
1038 
1039  m_last_seen.lock();
1040  m_last_seen[id] = now.in_msec();
1041  m_last_seen.unlock();
1042 }
1043 
1044 } // end namespace fawkes
long in_msec() const
Convert the stored time into milli-seconds.
Definition: time.cpp:242
Blue goal.
Definition: enums.h:64
A homogeneous representation of a polar coordinate.
Definition: hom_polar.h:31
A general matrix class.
Definition: matrix.h:33
This is supposed to be the central clock in Fawkes.
Definition: clock.h:34
A 2-dimensional pose, i.e.
Definition: hom_pose_2d.h:33
A class for handling time.
Definition: time.h:91
First half.
Definition: enums.h:71
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:70
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
Definition: enums.cpp:75
A homogeneous point.
Definition: hom_point.h:33
const char * worldinfo_msl_gamestate_tostring(worldinfo_msl_gamestate_t gamestate)
Convert MSL gamestate to a string.
Definition: enums.cpp:35
Both teams.
Definition: enums.h:58
Frozen, nothing moves.
Definition: enums.h:31
float yaw() const
Get the angle of the current orientation [0...2pi].
worldinfo_msl_gamestate_t
Game states for RoboCup MSL.
Definition: enums.h:30
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
Definition: enums.cpp:92
A homogeneous vector.
Definition: hom_vector.h:31
Cyan team.
Definition: enums.h:56
worldinfo_gamestate_team_t
Team.
Definition: enums.h:54
float x() const
Get the x-coordinate of the position.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
Container struct for momentary game state infos.
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
Definition: enums.cpp:108
float y() const
Get the y-coordinate of the position.
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:63
System ran out of memory and desired operation could not be fulfilled.
Definition: system.h:32