Fawkes API Fawkes Development Version
|
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