qa_worldinfo.cpp

00001 
00002 /***************************************************************************
00003  *  qa_worldinfo.cpp - Fawkes QA WorldInfoTransceiver
00004  *
00005  *  Created: Thu May 03 16:14:59 2007
00006  *  Copyright  2006-2007  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 /// @cond QA
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( /* block = */ 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 /// @endcond

Generated on Tue Feb 22 13:32:28 2011 for Fawkes API by  doxygen 1.4.7