Fawkes API  Fawkes Development Version
qa_worldinfo.cpp
1 
2 /***************************************************************************
3  * qa_worldinfo.cpp - Fawkes QA WorldInfoTransceiver
4  *
5  * Created: Thu May 03 16:14:59 2007
6  * Copyright 2006-2007 Tim Niemueller [www.niemueller.de]
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. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 /// @cond QA
25 
26 #include <core/threading/thread.h>
27 #include <netcomm/worldinfo/transceiver.h>
28 #ifdef HAVE_AVAHI
29 #include <netcomm/dns-sd/avahi_thread.h>
30 #endif
31 #include <netcomm/utils/resolver.h>
32 #include <utils/system/signal.h>
33 #include <utils/system/argparser.h>
34 
35 #include <netdb.h>
36 #include <cstdlib>
37 #include <cstdio>
38 #include <cstring>
39 #include <iostream>
40 
41 using namespace std;
42 using namespace fawkes;
43 
44 class WorldInfoSenderThread : public Thread
45 {
46 public:
47  WorldInfoSenderThread(unsigned short int port, bool loop, NetworkNameResolver *rs)
48  : Thread("WorldInfoSenderThread", Thread::OPMODE_CONTINUOUS)
49  {
50  i = 0;
51  try {
52  t = new WorldInfoTransceiver(WorldInfoTransceiver::MULTICAST,
53  "224.16.0.1", port,
54  "AllemaniACsX", "DoesAnyOneCare",
55  rs);
56  t->set_loop( loop );
57  } catch (Exception &e) {
58  e.print_trace();
59  throw;
60  }
61  covariance = (float *)malloc(WORLDINFO_COVARIANCE_SIZE_3X3 * sizeof(float));
62  for (unsigned int j = 0; j < WORLDINFO_COVARIANCE_SIZE_3X3; ++j) {
63  covariance[j] = j;
64  }
65  }
66 
67  ~WorldInfoSenderThread()
68  {
69  printf("Closing sender\n");
70  delete t;
71  free(covariance);
72  }
73 
74  virtual void loop()
75  {
76  printf("Sending %u\n", i);
77  t->set_pose(i, i+1, i+2, covariance);
78  t->set_velocity(i+3, i+4, i+5, covariance);
79  t->set_ball_pos(i+6, i+7, i+8, covariance);
80  t->set_ball_visible(i % 2 == 0, (i % 2 == 0 ? -1 : 1) * i+9);
81  t->set_ball_velocity(i+9, i+10, i+11, covariance);
82  t->add_opponent(i+12, i+13, i+14, covariance);
83  t->add_opponent(i+15, i+16, i+17, covariance);
84  t->add_disappeared_opponent(i+18);
85  t->add_disappeared_opponent(i+19);
86  t->set_gamestate(GS_FROZEN, TEAM_BOTH);
87  t->send();
88  ++i;
89  }
90 
91  private:
92  WorldInfoTransceiver *t;
93  unsigned int i;
94  float *covariance;
95 };
96 
97 
98 class WorldInfoReceiverThread : public Thread, public WorldInfoHandler
99 {
100 public:
101  WorldInfoReceiverThread(unsigned short int port, unsigned int max_num_msgs,
103  : Thread("WorldInfoReceiverThread", Thread::OPMODE_CONTINUOUS)
104  {
105  this->max_num_msgs = max_num_msgs;
106  try {
107  t = new WorldInfoTransceiver(WorldInfoTransceiver::MULTICAST,
108  "224.16.0.1", port,
109  "AllemaniACs", "WorldInfoQA",
110  rs);
111  t->add_handler(this);
112  } catch (Exception &e) {
113  e.print_trace();
114  throw;
115  }
116  }
117 
118  ~WorldInfoReceiverThread()
119  {
120  printf("Closing receiver\n");
121  delete t;
122  }
123 
124  virtual void loop()
125  {
126  printf("Waiting for data\n");
127  t->flush_sequence_numbers(10);
128  t->recv( /* block = */ true, max_num_msgs );
129  }
130 
131  virtual void pose_rcvd(const char *from_host,
132  float x, float y, float theta,
133  float *covariance)
134  {
135  cout << "Pose[" << from_host << "]: (x,y,th)=("
136  << x << "," << y << "," << theta << "), cov=(";
137  for ( unsigned int i = 0; i < WORLDINFO_COVARIANCE_SIZE_3X3; ++i) {
138  cout << covariance[i];
139  if ( i != WORLDINFO_COVARIANCE_SIZE_3X3 - 1 ) {
140  cout << ",";
141  }
142  }
143  cout << ")" << endl;
144  }
145 
146  virtual void velocity_rcvd(const char *from_host, float vel_x,
147  float vel_y, float vel_theta, float *covariance)
148  {
149  cout << "Velo[" << from_host << "]: (vx,vy,vth)=("
150  << vel_x << "," << vel_y << "," << vel_theta << ")" << endl;
151  }
152 
153  virtual void ball_pos_rcvd(const char *from_host,
154  bool visible, int visibility_history,
155  float dist, float bearing, float slope,
156  float *covariance)
157  {
158  printf("Ball[%s]: vis: %i vishis: %i (d,b,s)=(%f,%f,%f) cov=(%f,%f,%f,%f,%f,%f,%f,%f,%f)\n",
159  from_host, visible, visibility_history, dist, bearing, slope,
160  covariance[0], covariance[1], covariance[2],
161  covariance[3], covariance[4], covariance[5],
162  covariance[6], covariance[7], covariance[8]);
163  }
164 
165  virtual void ball_velocity_rcvd(const char *from_host,
166  float vel_x, float vel_y, float vel_z, float *covariance)
167  {
168  cout << "BVel[" << from_host << "]: (vx,vy,vz)=("
169  << vel_x << "," << vel_y << "," << vel_z << ")" << endl;
170  }
171 
172  virtual void opponent_pose_rcvd(const char *from_host, unsigned int uid,
173  float distance, float bearing, float *covariance)
174  {
175  printf("Oppt[%s]: (uid,d,b)=(%u,%f,%f) cov=(%f,%f,%f,%f)\n",
176  from_host, uid, distance, bearing,
177  covariance[0], covariance[1], covariance[2], covariance[3] );
178  }
179 
180 
181  virtual void opponent_disapp_rcvd(const char *from_host, unsigned int uid)
182  {
183  printf("OpptDisapp[%s]: uid=%u\n", from_host, uid);
184  }
185 
186  virtual void gamestate_rcvd(const char *from_host,
187  worldinfo_gamestate_t game_state,
188  worldinfo_gamestate_team_t state_team,
189  unsigned int score_cyan, unsigned int score_magenta,
191  worldinfo_gamestate_goalcolor_t our_goal_color,
193  {
194  printf("Gamestate[%s]: gs=%s gs_team=%s score: %u:%u our_team: %s our_goal: %s half: %s\n",
195  from_host,
196  worldinfo_gamestate_tostring(game_state),
198  score_cyan, score_magenta,
202 
203  }
204 
205  private:
206  WorldInfoTransceiver *t;
207  unsigned int max_num_msgs;
208 };
209 
210 
211 class WorldInfoQAMain : public SignalHandler
212 {
213  public:
214  WorldInfoQAMain(ArgumentParser *argp)
215  {
216 #ifdef HAVE_AVAHI
217  if ( argp->has_arg("a") ) {
218  at = new AvahiThread();
219  at->start();
220  printf("Waiting for Avahi thread to initialize\n");
221  at->wait_initialized();
222  } else {
223  at = NULL;
224  }
225  rs = new NetworkNameResolver(at);
226 #else
227  rs = new NetworkNameResolver();
228 #endif
229  s = NULL;
230  r = NULL;
231  this->argp = argp;
232  if ( argp->has_arg("r") ) {
233  printf("Going to be a receiver\n");
234  r = new WorldInfoReceiverThread(2806, argp->has_arg("s") ? 1 : 0, rs);
235  } else {
236  s = new WorldInfoSenderThread(2806, argp->has_arg("l"), rs);
237  }
238  }
239 
240  ~WorldInfoQAMain()
241  {
242 #ifdef HAVE_AVAHI
243  if ( at != NULL ) {
244  at->cancel();
245  at->join();
246  delete at;
247  }
248 #endif
249  delete s;
250  delete r;
251  }
252 
253 
254  virtual void handle_signal(int signum)
255  {
256  printf("Signal received, cancelling threads\n");
257  if ( s != NULL ) s->cancel();
258  if ( r != NULL ) r->cancel();
259  printf("Threads cancelled\n");
260  }
261 
262  void run()
263  {
264  if ( s != NULL ) {
265  s->start();
266  s->join();
267  }
268  if ( r != NULL ) {
269  r->start();
270  r->join();
271  }
272  }
273 
274  private:
275  ArgumentParser *argp;
276  WorldInfoSenderThread *s;
277  WorldInfoReceiverThread *r;
279 #ifdef HAVE_AVAHI
280  AvahiThread *at;
281 #endif
282 };
283 
284 int
285 main(int argc, char **argv)
286 {
287  ArgumentParser *argp = new ArgumentParser(argc, argv, "arlsh");
288 
289  if ( argp->has_arg("h") ) {
290  cout << "Usage: " << argv[0] << "[-r] [-h] [-s] [-l] [-a]" << endl
291  << " -r receiver (sender otherwise)" << endl
292  << " -h this help message" << endl
293  << " -s single per recv, only process a single message per recv()" << endl
294 #ifdef HAVE_AVAHI
295  << " -a enable Avahi for mDNS lookup" << endl
296 #else
297  << " -a not available (Avahi not installed)" << endl
298 #endif
299  << " -l enable multicast loop back" << endl;
300  return 0;
301  }
302 
303  WorldInfoQAMain m(argp);
304  SignalManager::register_handler(SIGINT, &m);
305  SignalManager::ignore(SIGPIPE);
306 
307  m.run();
308 
309  SignalManager::finalize();
310 
311  delete argp;
312  return 0;
313 }
314 
315 /// @endcond
Fawkes library namespace.
STL namespace.
Interface for signal handling.
Definition: signal.h:35
Parse command line arguments.
Definition: argparser.h:66
Thread class encapsulation of pthreads.
Definition: thread.h:42
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
Base class for exceptions in Fawkes.
Definition: exception.h:36
Avahi main thread.
Definition: avahi_thread.h:54
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
Definition: enums.cpp:92
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
Network name and address resolver.
Definition: resolver.h:48
worldinfo_gamestate_team_t
Team.
Definition: enums.h:54
bool has_arg(const char *argn)
Check if argument has been supplied.
Definition: argparser.cpp:169
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
Definition: enums.cpp:108
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:63