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/signal.h> 33 #include <utils/system/argparser.h> 44 class WorldInfoSenderThread :
public Thread 48 :
Thread(
"WorldInfoSenderThread",
Thread::OPMODE_CONTINUOUS)
52 t =
new WorldInfoTransceiver(WorldInfoTransceiver::MULTICAST,
54 "AllemaniACsX",
"DoesAnyOneCare",
61 covariance = (
float *)malloc(WORLDINFO_COVARIANCE_SIZE_3X3 *
sizeof(
float));
62 for (
unsigned int j = 0; j < WORLDINFO_COVARIANCE_SIZE_3X3; ++j) {
67 ~WorldInfoSenderThread()
69 printf(
"Closing sender\n");
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);
92 WorldInfoTransceiver *t;
98 class WorldInfoReceiverThread :
public Thread,
public WorldInfoHandler
101 WorldInfoReceiverThread(
unsigned short int port,
unsigned int max_num_msgs,
103 :
Thread(
"WorldInfoReceiverThread",
Thread::OPMODE_CONTINUOUS)
105 this->max_num_msgs = max_num_msgs;
107 t =
new WorldInfoTransceiver(WorldInfoTransceiver::MULTICAST,
109 "AllemaniACs",
"WorldInfoQA",
111 t->add_handler(
this);
118 ~WorldInfoReceiverThread()
120 printf(
"Closing receiver\n");
126 printf(
"Waiting for data\n");
127 t->flush_sequence_numbers(10);
128 t->recv(
true, max_num_msgs );
131 virtual void pose_rcvd(
const char *from_host,
132 float x,
float y,
float theta,
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 ) {
146 virtual void velocity_rcvd(
const char *from_host,
float vel_x,
147 float vel_y,
float vel_theta,
float *covariance)
149 cout <<
"Velo[" << from_host <<
"]: (vx,vy,vth)=(" 150 << vel_x <<
"," << vel_y <<
"," << vel_theta <<
")" << endl;
153 virtual void ball_pos_rcvd(
const char *from_host,
154 bool visible,
int visibility_history,
155 float dist,
float bearing,
float slope,
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]);
165 virtual void ball_velocity_rcvd(
const char *from_host,
166 float vel_x,
float vel_y,
float vel_z,
float *covariance)
168 cout <<
"BVel[" << from_host <<
"]: (vx,vy,vz)=(" 169 << vel_x <<
"," << vel_y <<
"," << vel_z <<
")" << endl;
172 virtual void opponent_pose_rcvd(
const char *from_host,
unsigned int uid,
173 float distance,
float bearing,
float *covariance)
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] );
181 virtual void opponent_disapp_rcvd(
const char *from_host,
unsigned int uid)
183 printf(
"OpptDisapp[%s]: uid=%u\n", from_host, uid);
186 virtual void gamestate_rcvd(
const char *from_host,
187 worldinfo_gamestate_t game_state,
189 unsigned int score_cyan,
unsigned int score_magenta,
194 printf(
"Gamestate[%s]: gs=%s gs_team=%s score: %u:%u our_team: %s our_goal: %s half: %s\n",
196 worldinfo_gamestate_tostring(game_state),
198 score_cyan, score_magenta,
206 WorldInfoTransceiver *t;
207 unsigned int max_num_msgs;
220 printf(
"Waiting for Avahi thread to initialize\n");
221 at->wait_initialized();
233 printf(
"Going to be a receiver\n");
234 r =
new WorldInfoReceiverThread(2806, argp->
has_arg(
"s") ? 1 : 0, rs);
236 s =
new WorldInfoSenderThread(2806, argp->
has_arg(
"l"), rs);
254 virtual void handle_signal(
int signum)
256 printf(
"Signal received, cancelling threads\n");
257 if ( s != NULL ) s->cancel();
258 if ( r != NULL ) r->cancel();
259 printf(
"Threads cancelled\n");
276 WorldInfoSenderThread *s;
277 WorldInfoReceiverThread *r;
285 main(
int argc,
char **argv)
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
295 <<
" -a enable Avahi for mDNS lookup" << endl
297 <<
" -a not available (Avahi not installed)" << endl
299 <<
" -l enable multicast loop back" << endl;
303 WorldInfoQAMain m(argp);
304 SignalManager::register_handler(SIGINT, &m);
305 SignalManager::ignore(SIGPIPE);
309 SignalManager::finalize();
Fawkes library namespace.
Interface for signal handling.
Parse command line arguments.
Thread class encapsulation of pthreads.
worldinfo_gamestate_half_t
Game time half.
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
Base class for exceptions in Fawkes.
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
void print_trace()
Prints trace to stderr.
Network name and address resolver.
worldinfo_gamestate_team_t
Team.
bool has_arg(const char *argn)
Check if argument has been supplied.
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
worldinfo_gamestate_goalcolor_t
Goal color.