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/argparser.h>
33 #include <utils/system/signal.h>
34 
35 #include <cstdio>
36 #include <cstdlib>
37 #include <cstring>
38 #include <iostream>
39 #include <netdb.h>
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(
53  WorldInfoTransceiver::MULTICAST, "224.16.0.1", port, "AllemaniACsX", "DoesAnyOneCare", rs);
54  t->set_loop(loop);
55  } catch (Exception &e) {
56  e.print_trace();
57  throw;
58  }
59  covariance = (float *)malloc(WORLDINFO_COVARIANCE_SIZE_3X3 * sizeof(float));
60  for (unsigned int j = 0; j < WORLDINFO_COVARIANCE_SIZE_3X3; ++j) {
61  covariance[j] = j;
62  }
63  }
64 
65  ~WorldInfoSenderThread()
66  {
67  printf("Closing sender\n");
68  delete t;
69  free(covariance);
70  }
71 
72  virtual void
73  loop()
74  {
75  printf("Sending %u\n", i);
76  t->set_pose(i, i + 1, i + 2, covariance);
77  t->set_velocity(i + 3, i + 4, i + 5, covariance);
78  t->set_ball_pos(i + 6, i + 7, i + 8, covariance);
79  t->set_ball_visible(i % 2 == 0, (i % 2 == 0 ? -1 : 1) * i + 9);
80  t->set_ball_velocity(i + 9, i + 10, i + 11, covariance);
81  t->add_opponent(i + 12, i + 13, i + 14, covariance);
82  t->add_opponent(i + 15, i + 16, i + 17, covariance);
83  t->add_disappeared_opponent(i + 18);
84  t->add_disappeared_opponent(i + 19);
85  t->set_gamestate(GS_FROZEN, TEAM_BOTH);
86  t->send();
87  ++i;
88  }
89 
90 private:
91  WorldInfoTransceiver *t;
92  unsigned int i;
93  float * covariance;
94 };
95 
96 class WorldInfoReceiverThread : public Thread, public WorldInfoHandler
97 {
98 public:
99  WorldInfoReceiverThread(unsigned short int port,
100  unsigned int max_num_msgs,
102  : Thread("WorldInfoReceiverThread", Thread::OPMODE_CONTINUOUS)
103  {
104  this->max_num_msgs = max_num_msgs;
105  try {
106  t = new WorldInfoTransceiver(
107  WorldInfoTransceiver::MULTICAST, "224.16.0.1", port, "AllemaniACs", "WorldInfoQA", rs);
108  t->add_handler(this);
109  } catch (Exception &e) {
110  e.print_trace();
111  throw;
112  }
113  }
114 
115  ~WorldInfoReceiverThread()
116  {
117  printf("Closing receiver\n");
118  delete t;
119  }
120 
121  virtual void
122  loop()
123  {
124  printf("Waiting for data\n");
125  t->flush_sequence_numbers(10);
126  t->recv(/* block = */ true, max_num_msgs);
127  }
128 
129  virtual void
130  pose_rcvd(const char *from_host, float x, float y, float theta, float *covariance)
131  {
132  cout << "Pose[" << from_host << "]: (x,y,th)=(" << x << "," << y << "," << theta << "), cov=(";
133  for (unsigned int i = 0; i < WORLDINFO_COVARIANCE_SIZE_3X3; ++i) {
134  cout << covariance[i];
135  if (i != WORLDINFO_COVARIANCE_SIZE_3X3 - 1) {
136  cout << ",";
137  }
138  }
139  cout << ")" << endl;
140  }
141 
142  virtual void
143  velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_theta, float *covariance)
144  {
145  cout << "Velo[" << from_host << "]: (vx,vy,vth)=(" << vel_x << "," << vel_y << "," << vel_theta
146  << ")" << endl;
147  }
148 
149  virtual void
150  ball_pos_rcvd(const char *from_host,
151  bool visible,
152  int visibility_history,
153  float dist,
154  float bearing,
155  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,
160  visible,
161  visibility_history,
162  dist,
163  bearing,
164  slope,
165  covariance[0],
166  covariance[1],
167  covariance[2],
168  covariance[3],
169  covariance[4],
170  covariance[5],
171  covariance[6],
172  covariance[7],
173  covariance[8]);
174  }
175 
176  virtual void
177  ball_velocity_rcvd(const char *from_host,
178  float vel_x,
179  float vel_y,
180  float vel_z,
181  float * covariance)
182  {
183  cout << "BVel[" << from_host << "]: (vx,vy,vz)=(" << vel_x << "," << vel_y << "," << vel_z
184  << ")" << endl;
185  }
186 
187  virtual void
188  opponent_pose_rcvd(const char * from_host,
189  unsigned int uid,
190  float distance,
191  float bearing,
192  float * covariance)
193  {
194  printf("Oppt[%s]: (uid,d,b)=(%u,%f,%f) cov=(%f,%f,%f,%f)\n",
195  from_host,
196  uid,
197  distance,
198  bearing,
199  covariance[0],
200  covariance[1],
201  covariance[2],
202  covariance[3]);
203  }
204 
205  virtual void
206  opponent_disapp_rcvd(const char *from_host, unsigned int uid)
207  {
208  printf("OpptDisapp[%s]: uid=%u\n", from_host, uid);
209  }
210 
211  virtual void
212  gamestate_rcvd(const char * from_host,
213  worldinfo_gamestate_t game_state,
214  worldinfo_gamestate_team_t state_team,
215  unsigned int score_cyan,
216  unsigned int score_magenta,
218  worldinfo_gamestate_goalcolor_t our_goal_color,
220  {
221  printf(
222  "Gamestate[%s]: gs=%s gs_team=%s score: %u:%u our_team: %s our_goal: %s half: %s\n",
223  from_host,
224  worldinfo_gamestate_tostring(game_state),
226  score_cyan,
227  score_magenta,
231  }
232 
233 private:
234  WorldInfoTransceiver *t;
235  unsigned int max_num_msgs;
236 };
237 
238 class WorldInfoQAMain : public SignalHandler
239 {
240 public:
241  WorldInfoQAMain(ArgumentParser *argp)
242  {
243 #ifdef HAVE_AVAHI
244  if (argp->has_arg("a")) {
245  at = new AvahiThread();
246  at->start();
247  printf("Waiting for Avahi thread to initialize\n");
248  at->wait_initialized();
249  } else {
250  at = NULL;
251  }
252  rs = new NetworkNameResolver(at);
253 #else
254  rs = new NetworkNameResolver();
255 #endif
256  s = NULL;
257  r = NULL;
258  this->argp = argp;
259  if (argp->has_arg("r")) {
260  printf("Going to be a receiver\n");
261  r = new WorldInfoReceiverThread(2806, argp->has_arg("s") ? 1 : 0, rs);
262  } else {
263  s = new WorldInfoSenderThread(2806, argp->has_arg("l"), rs);
264  }
265  }
266 
267  ~WorldInfoQAMain()
268  {
269 #ifdef HAVE_AVAHI
270  if (at != NULL) {
271  at->cancel();
272  at->join();
273  delete at;
274  }
275 #endif
276  delete s;
277  delete r;
278  }
279 
280  virtual void
281  handle_signal(int signum)
282  {
283  printf("Signal received, cancelling threads\n");
284  if (s != NULL)
285  s->cancel();
286  if (r != NULL)
287  r->cancel();
288  printf("Threads cancelled\n");
289  }
290 
291  void
292  run()
293  {
294  if (s != NULL) {
295  s->start();
296  s->join();
297  }
298  if (r != NULL) {
299  r->start();
300  r->join();
301  }
302  }
303 
304 private:
305  ArgumentParser * argp;
306  WorldInfoSenderThread * s;
307  WorldInfoReceiverThread *r;
308  NetworkNameResolver * rs;
309 #ifdef HAVE_AVAHI
310  AvahiThread *at;
311 #endif
312 };
313 
314 int
315 main(int argc, char **argv)
316 {
317  ArgumentParser *argp = new ArgumentParser(argc, argv, "arlsh");
318 
319  if (argp->has_arg("h")) {
320  cout << "Usage: " << argv[0] << "[-r] [-h] [-s] [-l] [-a]" << endl
321  << " -r receiver (sender otherwise)" << endl
322  << " -h this help message" << endl
323  << " -s single per recv, only process a single message per recv()" << endl
324 #ifdef HAVE_AVAHI
325  << " -a enable Avahi for mDNS lookup" << endl
326 #else
327  << " -a not available (Avahi not installed)" << endl
328 #endif
329  << " -l enable multicast loop back" << endl;
330  return 0;
331  }
332 
333  WorldInfoQAMain m(argp);
334  SignalManager::register_handler(SIGINT, &m);
335  SignalManager::ignore(SIGPIPE);
336 
337  m.run();
338 
339  SignalManager::finalize();
340 
341  delete argp;
342  return 0;
343 }
344 
345 /// @endcond
Parse command line arguments.
Definition: argparser.h:64
bool has_arg(const char *argn)
Check if argument has been supplied.
Definition: argparser.cpp:165
Avahi main thread.
Definition: avahi_thread.h:55
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:601
Network name and address resolver.
Definition: resolver.h:45
Interface for signal handling.
Definition: signal.h:36
Thread class encapsulation of pthreads.
Definition: thread.h:46
Fawkes library namespace.
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
Definition: enums.cpp:91
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:61
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
Definition: enums.cpp:105
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
Definition: enums.cpp:75
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:67
worldinfo_gamestate_team_t
Team.
Definition: enums.h:53