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