26 #include <core/threading/thread.h>
27 #include <netcomm/worldinfo/transceiver.h>
29 # include <netcomm/dns-sd/avahi_thread.h>
31 #include <netcomm/utils/resolver.h>
32 #include <utils/system/argparser.h>
33 #include <utils/system/signal.h>
44 class WorldInfoSenderThread :
public Thread
48 :
Thread(
"WorldInfoSenderThread",
Thread::OPMODE_CONTINUOUS)
52 t =
new WorldInfoTransceiver(
53 WorldInfoTransceiver::MULTICAST,
"224.16.0.1", port,
"AllemaniACsX",
"DoesAnyOneCare", rs);
59 covariance = (
float *)malloc(WORLDINFO_COVARIANCE_SIZE_3X3 *
sizeof(
float));
60 for (
unsigned int j = 0; j < WORLDINFO_COVARIANCE_SIZE_3X3; ++j) {
65 ~WorldInfoSenderThread()
67 printf(
"Closing sender\n");
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);
91 WorldInfoTransceiver *t;
96 class WorldInfoReceiverThread :
public Thread,
public WorldInfoHandler
99 WorldInfoReceiverThread(
unsigned short int port,
100 unsigned int max_num_msgs,
102 :
Thread(
"WorldInfoReceiverThread",
Thread::OPMODE_CONTINUOUS)
104 this->max_num_msgs = max_num_msgs;
106 t =
new WorldInfoTransceiver(
107 WorldInfoTransceiver::MULTICAST,
"224.16.0.1", port,
"AllemaniACs",
"WorldInfoQA", rs);
108 t->add_handler(
this);
115 ~WorldInfoReceiverThread()
117 printf(
"Closing receiver\n");
124 printf(
"Waiting for data\n");
125 t->flush_sequence_numbers(10);
126 t->recv(
true, max_num_msgs);
130 pose_rcvd(
const char *from_host,
float x,
float y,
float theta,
float *covariance)
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) {
143 velocity_rcvd(
const char *from_host,
float vel_x,
float vel_y,
float vel_theta,
float *covariance)
145 cout <<
"Velo[" << from_host <<
"]: (vx,vy,vth)=(" << vel_x <<
"," << vel_y <<
"," << vel_theta
150 ball_pos_rcvd(
const char *from_host,
152 int visibility_history,
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",
177 ball_velocity_rcvd(
const char *from_host,
183 cout <<
"BVel[" << from_host <<
"]: (vx,vy,vz)=(" << vel_x <<
"," << vel_y <<
"," << vel_z
188 opponent_pose_rcvd(
const char * from_host,
194 printf(
"Oppt[%s]: (uid,d,b)=(%u,%f,%f) cov=(%f,%f,%f,%f)\n",
206 opponent_disapp_rcvd(
const char *from_host,
unsigned int uid)
208 printf(
"OpptDisapp[%s]: uid=%u\n", from_host, uid);
212 gamestate_rcvd(
const char * from_host,
213 worldinfo_gamestate_t game_state,
215 unsigned int score_cyan,
216 unsigned int score_magenta,
222 "Gamestate[%s]: gs=%s gs_team=%s score: %u:%u our_team: %s our_goal: %s half: %s\n",
224 worldinfo_gamestate_tostring(game_state),
234 WorldInfoTransceiver *t;
235 unsigned int max_num_msgs;
247 printf(
"Waiting for Avahi thread to initialize\n");
248 at->wait_initialized();
260 printf(
"Going to be a receiver\n");
261 r =
new WorldInfoReceiverThread(2806, argp->
has_arg(
"s") ? 1 : 0, rs);
263 s =
new WorldInfoSenderThread(2806, argp->
has_arg(
"l"), rs);
281 handle_signal(
int signum)
283 printf(
"Signal received, cancelling threads\n");
288 printf(
"Threads cancelled\n");
306 WorldInfoSenderThread * s;
307 WorldInfoReceiverThread *r;
315 main(
int argc,
char **argv)
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
325 <<
" -a enable Avahi for mDNS lookup" << endl
327 <<
" -a not available (Avahi not installed)" << endl
329 <<
" -l enable multicast loop back" << endl;
333 WorldInfoQAMain m(argp);
334 SignalManager::register_handler(SIGINT, &m);
335 SignalManager::ignore(SIGPIPE);
339 SignalManager::finalize();
Parse command line arguments.
bool has_arg(const char *argn)
Check if argument has been supplied.
Base class for exceptions in Fawkes.
void print_trace() noexcept
Prints trace to stderr.
Network name and address resolver.
Interface for signal handling.
Thread class encapsulation of pthreads.
Fawkes library namespace.
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
worldinfo_gamestate_goalcolor_t
Goal color.
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
worldinfo_gamestate_half_t
Game time half.
worldinfo_gamestate_team_t
Team.