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
41using namespace std;
42using namespace fawkes;
43
44class WorldInfoSenderThread : public Thread
45{
46public:
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
90private:
91 WorldInfoTransceiver *t;
92 unsigned int i;
93 float * covariance;
94};
95
96class WorldInfoReceiverThread : public Thread, public WorldInfoHandler
97{
98public:
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,
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
233private:
234 WorldInfoTransceiver *t;
235 unsigned int max_num_msgs;
236};
237
238class WorldInfoQAMain : public SignalHandler
239{
240public:
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
304private:
305 ArgumentParser * argp;
306 WorldInfoSenderThread * s;
307 WorldInfoReceiverThread *r;
309#ifdef HAVE_AVAHI
310 AvahiThread *at;
311#endif
312};
313
314int
315main(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() noexcept
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