00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <core/threading/thread.h>
00027 #include <netcomm/worldinfo/transceiver.h>
00028 #ifdef HAVE_AVAHI
00029 #include <netcomm/dns-sd/avahi_thread.h>
00030 #endif
00031 #include <netcomm/utils/resolver.h>
00032 #include <utils/system/signal.h>
00033 #include <utils/system/argparser.h>
00034
00035 #include <netdb.h>
00036 #include <cstdlib>
00037 #include <cstdio>
00038 #include <cstring>
00039 #include <iostream>
00040
00041 using namespace std;
00042 using namespace fawkes;
00043
00044 class WorldInfoSenderThread : public Thread
00045 {
00046 public:
00047 WorldInfoSenderThread(unsigned short int port, bool loop, NetworkNameResolver *rs)
00048 : Thread("WorldInfoSenderThread", Thread::OPMODE_CONTINUOUS)
00049 {
00050 i = 0;
00051 try {
00052 t = new WorldInfoTransceiver("224.16.0.1", port,
00053 "AllemaniACsX", "DoesAnyOneCare",
00054 rs);
00055 t->set_loop( loop );
00056 } catch (Exception &e) {
00057 e.print_trace();
00058 throw;
00059 }
00060 covariance = (float *)malloc(WORLDINFO_COVARIANCE_SIZE_3X3 * sizeof(float));
00061 for (unsigned int j = 0; j < WORLDINFO_COVARIANCE_SIZE_3X3; ++j) {
00062 covariance[j] = j;
00063 }
00064 }
00065
00066 ~WorldInfoSenderThread()
00067 {
00068 printf("Closing sender\n");
00069 delete t;
00070 free(covariance);
00071 }
00072
00073 virtual void loop()
00074 {
00075 printf("Sending %u\n", i);
00076 t->set_pose(i, i+1, i+2, covariance);
00077 t->set_velocity(i+3, i+4, i+5, covariance);
00078 t->set_ball_pos(i+6, i+7, i+8, covariance);
00079 t->set_ball_visible(i % 2 == 0, (i % 2 == 0 ? -1 : 1) * i+9);
00080 t->set_ball_velocity(i+9, i+10, i+11, covariance);
00081 t->add_opponent(i+12, i+13, i+14, covariance);
00082 t->add_opponent(i+15, i+16, i+17, covariance);
00083 t->add_disappeared_opponent(i+18);
00084 t->add_disappeared_opponent(i+19);
00085 t->set_gamestate(GS_FROZEN, TEAM_BOTH);
00086 t->send();
00087 ++i;
00088 }
00089
00090 private:
00091 WorldInfoTransceiver *t;
00092 unsigned int i;
00093 float *covariance;
00094 };
00095
00096
00097 class WorldInfoReceiverThread : public Thread, public WorldInfoHandler
00098 {
00099 public:
00100 WorldInfoReceiverThread(unsigned short int port, unsigned int max_num_msgs,
00101 NetworkNameResolver *rs)
00102 : Thread("WorldInfoReceiverThread", Thread::OPMODE_CONTINUOUS)
00103 {
00104 this->max_num_msgs = max_num_msgs;
00105 try {
00106 t = new WorldInfoTransceiver("224.16.0.1", port,
00107 "AllemaniACs", "WorldInfoQA",
00108 rs);
00109 t->add_handler(this);
00110 } catch (Exception &e) {
00111 e.print_trace();
00112 throw;
00113 }
00114 }
00115
00116 ~WorldInfoReceiverThread()
00117 {
00118 printf("Closing receiver\n");
00119 delete t;
00120 }
00121
00122 virtual void loop()
00123 {
00124 printf("Waiting for data\n");
00125 t->flush_sequence_numbers(10);
00126 t->recv( true, max_num_msgs );
00127 }
00128
00129 virtual void pose_rcvd(const char *from_host,
00130 float x, float y, float theta,
00131 float *covariance)
00132 {
00133 cout << "Pose[" << from_host << "]: (x,y,th)=("
00134 << x << "," << y << "," << theta << "), cov=(";
00135 for ( unsigned int i = 0; i < WORLDINFO_COVARIANCE_SIZE_3X3; ++i) {
00136 cout << covariance[i];
00137 if ( i != WORLDINFO_COVARIANCE_SIZE_3X3 - 1 ) {
00138 cout << ",";
00139 }
00140 }
00141 cout << ")" << endl;
00142 }
00143
00144 virtual void velocity_rcvd(const char *from_host, float vel_x,
00145 float vel_y, float vel_theta, float *covariance)
00146 {
00147 cout << "Velo[" << from_host << "]: (vx,vy,vth)=("
00148 << vel_x << "," << vel_y << "," << vel_theta << ")" << endl;
00149 }
00150
00151 virtual void ball_pos_rcvd(const char *from_host,
00152 bool visible, int visibility_history,
00153 float dist, float bearing, float slope,
00154 float *covariance)
00155 {
00156 printf("Ball[%s]: vis: %i vishis: %i (d,b,s)=(%f,%f,%f) cov=(%f,%f,%f,%f,%f,%f,%f,%f,%f)\n",
00157 from_host, visible, visibility_history, dist, bearing, slope,
00158 covariance[0], covariance[1], covariance[2],
00159 covariance[3], covariance[4], covariance[5],
00160 covariance[6], covariance[7], covariance[8]);
00161 }
00162
00163 virtual void ball_velocity_rcvd(const char *from_host,
00164 float vel_x, float vel_y, float vel_z, float *covariance)
00165 {
00166 cout << "BVel[" << from_host << "]: (vx,vy,vz)=("
00167 << vel_x << "," << vel_y << "," << vel_z << ")" << endl;
00168 }
00169
00170 virtual void opponent_pose_rcvd(const char *from_host, unsigned int uid,
00171 float distance, float bearing, float *covariance)
00172 {
00173 printf("Oppt[%s]: (uid,d,b)=(%u,%f,%f) cov=(%f,%f,%f,%f)\n",
00174 from_host, uid, distance, bearing,
00175 covariance[0], covariance[1], covariance[2], covariance[3] );
00176 }
00177
00178
00179 virtual void opponent_disapp_rcvd(const char *from_host, unsigned int uid)
00180 {
00181 printf("OpptDisapp[%s]: uid=%u\n", from_host, uid);
00182 }
00183
00184 virtual void gamestate_rcvd(const char *from_host,
00185 worldinfo_gamestate_t game_state,
00186 worldinfo_gamestate_team_t state_team,
00187 unsigned int score_cyan, unsigned int score_magenta,
00188 worldinfo_gamestate_team_t our_team,
00189 worldinfo_gamestate_goalcolor_t our_goal_color,
00190 worldinfo_gamestate_half_t half)
00191 {
00192 printf("Gamestate[%s]: gs=%s gs_team=%s score: %u:%u our_team: %s our_goal: %s half: %s\n",
00193 from_host,
00194 worldinfo_gamestate_tostring(game_state),
00195 worldinfo_gamestate_team_tostring(state_team),
00196 score_cyan, score_magenta,
00197 worldinfo_gamestate_team_tostring(our_team),
00198 worldinfo_gamestate_goalcolor_tostring(our_goal_color),
00199 worldinfo_gamestate_half_tostring(half));
00200
00201 }
00202
00203 private:
00204 WorldInfoTransceiver *t;
00205 unsigned int max_num_msgs;
00206 };
00207
00208
00209 class WorldInfoQAMain : public SignalHandler
00210 {
00211 public:
00212 WorldInfoQAMain(ArgumentParser *argp)
00213 {
00214 #ifdef HAVE_AVAHI
00215 if ( argp->has_arg("a") ) {
00216 at = new AvahiThread();
00217 at->start();
00218 printf("Waiting for Avahi thread to initialize\n");
00219 at->wait_initialized();
00220 } else {
00221 at = NULL;
00222 }
00223 rs = new NetworkNameResolver(at);
00224 #else
00225 rs = new NetworkNameResolver();
00226 #endif
00227 s = NULL;
00228 r = NULL;
00229 this->argp = argp;
00230 if ( argp->has_arg("r") ) {
00231 printf("Going to be a receiver\n");
00232 r = new WorldInfoReceiverThread(2806, argp->has_arg("s") ? 1 : 0, rs);
00233 } else {
00234 s = new WorldInfoSenderThread(2806, argp->has_arg("l"), rs);
00235 }
00236 }
00237
00238 ~WorldInfoQAMain()
00239 {
00240 #ifdef HAVE_AVAHI
00241 if ( at != NULL ) {
00242 at->cancel();
00243 at->join();
00244 delete at;
00245 }
00246 #endif
00247 delete s;
00248 delete r;
00249 }
00250
00251
00252 virtual void handle_signal(int signum)
00253 {
00254 printf("Signal received, cancelling threads\n");
00255 if ( s != NULL ) s->cancel();
00256 if ( r != NULL ) r->cancel();
00257 printf("Threads cancelled\n");
00258 }
00259
00260 void run()
00261 {
00262 if ( s != NULL ) {
00263 s->start();
00264 s->join();
00265 }
00266 if ( r != NULL ) {
00267 r->start();
00268 r->join();
00269 }
00270 }
00271
00272 private:
00273 ArgumentParser *argp;
00274 WorldInfoSenderThread *s;
00275 WorldInfoReceiverThread *r;
00276 NetworkNameResolver *rs;
00277 #ifdef HAVE_AVAHI
00278 AvahiThread *at;
00279 #endif
00280 };
00281
00282 int
00283 main(int argc, char **argv)
00284 {
00285 ArgumentParser *argp = new ArgumentParser(argc, argv, "arlsh");
00286
00287 if ( argp->has_arg("h") ) {
00288 cout << "Usage: " << argv[0] << "[-r] [-h] [-s] [-l] [-a]" << endl
00289 << " -r receiver (sender otherwise)" << endl
00290 << " -h this help message" << endl
00291 << " -s single per recv, only process a single message per recv()" << endl
00292 #ifdef HAVE_AVAHI
00293 << " -a enable Avahi for mDNS lookup" << endl
00294 #else
00295 << " -a not available (Avahi not installed)" << endl
00296 #endif
00297 << " -l enable multicast loop back" << endl;
00298 return 0;
00299 }
00300
00301 WorldInfoQAMain m(argp);
00302 SignalManager::register_handler(SIGINT, &m);
00303 SignalManager::ignore(SIGPIPE);
00304
00305 m.run();
00306
00307 SignalManager::finalize();
00308
00309 delete argp;
00310 return 0;
00311 }
00312
00313
