Fawkes API  Fawkes Development Version
net_thread.cpp
00001 
00002 /***************************************************************************
00003  *  net_thread.cpp - Fawkes WorldModel Plugin Network Thread
00004  *
00005  *  Created: Fri Jun 29 16:56:15 2007 (on flight to RoboCup 2007, Atlanta)
00006  *  Copyright  2006-2009  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.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "net_thread.h"
00024 
00025 #include <netcomm/worldinfo/transceiver.h>
00026 #include <interfaces/ObjectPositionInterface.h>
00027 #include <interfaces/GameStateInterface.h>
00028 
00029 #include <string>
00030 #include <cstring>
00031 #include <cstdlib>
00032 #include <cstdio>
00033 
00034 using namespace std;
00035 using namespace fawkes;
00036 
00037 /** @class WorldModelNetworkThread "net_thread.h"
00038  * Network thread of worldmodel plugin.
00039  * @author Tim Niemueller
00040  */
00041 
00042 /** Constructor.
00043  */
00044 WorldModelNetworkThread::WorldModelNetworkThread()
00045   : Thread("WorldModelNetworkThread", Thread::OPMODE_CONTINUOUS)
00046 {
00047   __worldinfo_transceiver = NULL;
00048   set_prepfin_conc_loop(true);
00049   __opponent_id = 0;
00050 }
00051 
00052 
00053 /** Destructor. */
00054 WorldModelNetworkThread::~WorldModelNetworkThread()
00055 {
00056 }
00057 
00058 
00059 void
00060 WorldModelNetworkThread::init()
00061 {
00062   std::string multicast_addr;
00063   unsigned int port;
00064   std::string encryption_key;
00065   std::string encryption_iv;
00066   bool        cfg_multicast_loopback;
00067   try {
00068     multicast_addr = config->get_string("/worldinfo/multicast_addr");
00069     port = config->get_uint("/worldinfo/udp_port");
00070     encryption_key = config->get_string("/worldinfo/encryption_key");
00071     encryption_iv  = config->get_string("/worldinfo/encryption_iv");
00072     __cfg_sleep_time_msec   = config->get_uint("/worldinfo/sleep_time_msec");
00073     __cfg_max_msgs_per_recv = config->get_uint("/worldinfo/max_msgs_per_recv");
00074     __cfg_flush_time_sec    = config->get_uint("/worldinfo/flush_time_sec");
00075     cfg_multicast_loopback  = config->get_bool("/worldinfo/multicast_loopback");
00076   } catch (Exception &e) {
00077     e.append("Could not get required configuration data for worldmodel");
00078     e.print_trace();
00079     throw;
00080   }
00081 
00082   __worldinfo_transceiver = new WorldInfoTransceiver(WorldInfoTransceiver::MULTICAST,
00083                                                      multicast_addr.c_str(), port,
00084                                                      encryption_key.c_str(), encryption_iv.c_str(),
00085                                                      nnresolver);
00086 
00087   __worldinfo_transceiver->add_handler(this);
00088   __worldinfo_transceiver->set_loop(cfg_multicast_loopback);
00089 
00090   try {
00091     __gamestate_if = blackboard->open_for_writing<GameStateInterface>("WI GameState");
00092   } catch (Exception &e) {
00093     delete __worldinfo_transceiver;
00094     e.print_trace();
00095     throw;
00096   }
00097 }
00098 
00099 
00100 void
00101 WorldModelNetworkThread::finalize()
00102 {
00103   // close all WI pose interfaces
00104   for (LockMap<string, ObjectPositionInterface*>::iterator i = __pose_ifs.begin();
00105        i != __pose_ifs.end();
00106        ++i) {
00107     blackboard->close(i->second);
00108   }
00109 
00110   // close all WI ball interfaces
00111   for (LockMap<string, ObjectPositionInterface*>::iterator i = __ball_ifs.begin();
00112        i != __ball_ifs.end();
00113        ++i) {
00114     blackboard->close(i->second);
00115   }
00116 
00117   // close all WI opponent interfaces
00118   for (LockMap<string, UidTimeObjPosMap>::iterator i = __opponent_ifs.begin();
00119        i != __opponent_ifs.end();
00120        ++i) {
00121     for (UidTimeObjPosMap::iterator j = i->second.begin();
00122          j != i->second.end();
00123          ++j) {
00124       blackboard->close(j->second.second);
00125     }
00126   }
00127 
00128   blackboard->close(__gamestate_if);
00129   delete __worldinfo_transceiver;
00130 }
00131 
00132 
00133 void
00134 WorldModelNetworkThread::loop()
00135 {
00136   __worldinfo_transceiver->flush_sequence_numbers(__cfg_flush_time_sec);
00137   __worldinfo_transceiver->recv(false, __cfg_max_msgs_per_recv);
00138   usleep( __cfg_sleep_time_msec * 1000 );
00139   // check for dead ones
00140   std::map<std::string, fawkes::Time>::iterator lsi = __last_seen.begin();
00141   Time now;
00142   __last_seen.lock();
00143   while (lsi != __last_seen.end()) {
00144     if (now - &lsi->second > 3.0) {
00145       logger->log_info("WorldModelNetworkThread", "Expiring host %s", lsi->first.c_str());
00146       // this is is as dead as the chair I'm sitting on
00147       __pose_ifs.lock();
00148       if (__pose_ifs.find(lsi->first) != __pose_ifs.end()) {
00149         blackboard->close(__pose_ifs[lsi->first]);
00150         __pose_ifs.erase(lsi->first);
00151       }
00152       __pose_ifs.unlock();
00153       __ball_ifs.lock();
00154       if (__ball_ifs.find(lsi->first) != __ball_ifs.end()) {
00155         blackboard->close(__ball_ifs[lsi->first]);
00156         __ball_ifs.erase(lsi->first);
00157       }
00158       __ball_ifs.unlock();
00159       __opponent_ifs.lock();
00160       if (__opponent_ifs.find(lsi->first) != __opponent_ifs.end()) {
00161         std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> >::iterator i;
00162         for (i = __opponent_ifs[lsi->first].begin(); i != __opponent_ifs[lsi->first].end(); ++i) {
00163           blackboard->close(i->second.second);
00164         }
00165         __opponent_ifs.erase(lsi->first);
00166       }
00167       __opponent_ifs.unlock();
00168       std::map<std::string, fawkes::Time>::iterator tmp = lsi;
00169       ++lsi;
00170       __last_seen.erase(tmp);
00171     } else {
00172       ++lsi;
00173     }
00174   }
00175   __last_seen.unlock();
00176 
00177   __opponent_ifs.lock();
00178   std::map<std::string, UidTimeObjPosMap>::iterator o = __opponent_ifs.begin();
00179   while (o != __opponent_ifs.end()) {
00180     UidTimeObjPosMap::iterator top = o->second.begin();
00181     while (top != o->second.end()) {
00182       if (now - &(top->second.first) > 3.0) {
00183         logger->log_info("WorldModelNetworkThread", "Expiring Opponent %s:%u", o->first.c_str(), top->first);
00184         blackboard->close(top->second.second);
00185         UidTimeObjPosMap::iterator tmp = top;
00186         ++top;
00187         o->second.erase(tmp);
00188       } else {
00189         ++top;
00190       }
00191     }
00192     if (o->second.empty()) {
00193       std::map<std::string, UidTimeObjPosMap>::iterator tmp = o;
00194       ++o;
00195       __opponent_ifs.erase(tmp);
00196     } else {
00197       ++o;
00198     }
00199   }
00200   __opponent_ifs.unlock();
00201 
00202 }
00203 
00204 
00205 /** Access the WI transceiver.
00206  * @return pointer to the WI transceiver
00207  */
00208 WorldInfoTransceiver*
00209 WorldModelNetworkThread::get_transceiver()
00210 {
00211   return __worldinfo_transceiver;
00212 }
00213 
00214 
00215 void
00216 WorldModelNetworkThread::pose_rcvd(const char *from_host,
00217                                    float x, float y, float theta,
00218                                    float *covariance)
00219 {
00220   __pose_ifs.lock();
00221   if (__pose_ifs.find(from_host) == __pose_ifs.end()) {
00222     try {
00223       std::string id = std::string("WI RoboPos ") + from_host;
00224       __pose_ifs[from_host] = blackboard->open_for_writing<ObjectPositionInterface>(id.c_str());
00225     } catch (Exception &e) {
00226       logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
00227                        "for pose of %s, exception follows", from_host);
00228       logger->log_warn("WorldModelNetworkThread", e);
00229       return;
00230     }
00231   }
00232 
00233   // Pose is our aliveness indicator
00234   __last_seen.lock();
00235   __last_seen[from_host].stamp();
00236   __last_seen.unlock();
00237 
00238   ObjectPositionInterface *iface = __pose_ifs[from_host];
00239   iface->set_world_x(x);
00240   iface->set_world_y(y);
00241   iface->set_world_z(theta);
00242   iface->set_world_xyz_covariance(covariance);
00243   iface->write();
00244   __pose_ifs.unlock();
00245 }
00246 
00247 
00248 void
00249 WorldModelNetworkThread::velocity_rcvd(const char *from_host, float vel_x,
00250                                        float vel_y, float vel_theta, float *covariance)
00251 {
00252   // TODO
00253 }
00254 
00255 
00256 void
00257 WorldModelNetworkThread::ball_pos_rcvd(const char *from_host,
00258                                        bool visible, int visibility_history,
00259                                        float dist, float bearing, float slope,
00260                                        float *covariance)
00261 {
00262   __ball_ifs.lock();
00263   if (__ball_ifs.find(from_host) == __ball_ifs.end()) {
00264     try {
00265       std::string id = std::string("WI BPos ") + from_host;
00266       __ball_ifs[from_host] = blackboard->open_for_writing<ObjectPositionInterface>(id.c_str());
00267     } catch (Exception &e) {
00268       logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
00269                        "for ball pos of %s, exception follows", from_host);
00270       logger->log_warn("WorldModelNetworkThread", e);
00271       return;
00272     }
00273   }
00274 
00275   ObjectPositionInterface *iface = __ball_ifs[from_host];
00276   iface->set_flags( iface->flags() |
00277                     ObjectPositionInterface::TYPE_BALL |
00278                     ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR |
00279                     ObjectPositionInterface::FLAG_HAS_COVARIANCES );
00280   iface->set_visible(visible);
00281   iface->set_visibility_history(visibility_history);
00282   iface->set_distance(dist);
00283   iface->set_bearing(bearing);
00284   iface->set_slope(slope);
00285   iface->set_dbs_covariance(covariance);
00286   iface->write();
00287   __ball_ifs.unlock();
00288 }
00289 
00290 
00291 void
00292 WorldModelNetworkThread::global_ball_pos_rcvd(const char *from_host,
00293                                               bool visible, int visibility_history,
00294                                               float x, float y, float z,
00295                                               float *covariance)
00296 {
00297   __ball_ifs.lock();
00298   if (__ball_ifs.find(from_host) == __ball_ifs.end()) {
00299     try {
00300       std::string id = std::string("WI BPos ") + from_host;
00301       __ball_ifs[from_host] = blackboard->open_for_writing<ObjectPositionInterface>(id.c_str());
00302     } catch (Exception &e) {
00303       logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
00304                        "for ball pos of %s, exception follows", from_host);
00305       logger->log_warn("WorldModelNetworkThread", e);
00306       return;
00307     }
00308   }
00309 
00310   ObjectPositionInterface *iface = __ball_ifs[from_host];
00311   iface->set_flags( iface->flags() | 
00312                     ObjectPositionInterface::TYPE_BALL |
00313                     ObjectPositionInterface::FLAG_HAS_WORLD |
00314                     ObjectPositionInterface::FLAG_HAS_Z_AS_ORI |
00315                     ObjectPositionInterface::FLAG_HAS_COVARIANCES );
00316   iface->set_visible(visible);
00317   iface->set_visibility_history(visibility_history);
00318   iface->set_world_x(x);
00319   iface->set_world_y(y);
00320   iface->set_world_z(z);
00321   iface->set_world_xyz_covariance(covariance);
00322   iface->write();
00323   __ball_ifs.unlock();
00324 }
00325 
00326 
00327 void
00328 WorldModelNetworkThread::ball_velocity_rcvd(const char *from_host,
00329                                             float vel_x, float vel_y, float vel_z,
00330                                             float *covariance)
00331 {
00332   // TODO
00333 }
00334 
00335 
00336 void
00337 WorldModelNetworkThread::global_ball_velocity_rcvd(const char *from_host,
00338                                                    float vel_x, float vel_y, float vel_z,
00339                                                    float *covariance)
00340 {
00341   // TODO
00342 }
00343 
00344 
00345 void
00346 WorldModelNetworkThread::opponent_pose_rcvd(const char *from_host,
00347                                             unsigned int uid,
00348                                             float distance, float bearing,
00349                                             float *covariance)
00350 {
00351   __opponent_ifs.lock();
00352   std::map<std::string, std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> > >::iterator f;
00353 
00354   bool iface_exists = true;
00355   if ( ((f = __opponent_ifs.find(from_host)) == __opponent_ifs.end()) ||
00356        (f->second.find(uid) == f->second.end()) ) {
00357 
00358     char *tmp;
00359     if (asprintf(&tmp, "WI Opp %u %s", ++__opponent_id, from_host) != -1) {
00360       try {
00361         std::string id = tmp;
00362         free(tmp);
00363         logger->log_debug("WorldModelNetworkThread", "Opening new interface for %s:%u", from_host, uid);
00364         __opponent_ifs[from_host][uid] = make_pair(Time(), blackboard->open_for_writing<ObjectPositionInterface>(id.c_str()));
00365       } catch (Exception &e) {
00366         logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
00367                          "for opponent %s:%u, exception follows", from_host, uid);
00368         logger->log_warn("WorldModelNetworkThread", e);
00369         iface_exists = false;
00370       }
00371     } else {
00372       logger->log_error("WorldModelNetworkThread", "Could not create interface ID string, out of memory during asprintf().");
00373       iface_exists = false;
00374     }
00375   }
00376 
00377   if (iface_exists) {
00378     logger->log_debug("WorldModelNetworkThread", "Setting opponent %s:%u", from_host, uid);
00379     ObjectPositionInterface *iface = __opponent_ifs[from_host][uid].second;
00380     iface->set_distance(distance);
00381     iface->set_bearing(bearing);
00382     iface->set_dbs_covariance(covariance);
00383     iface->write();
00384 
00385     __opponent_ifs[from_host][uid].first.stamp();
00386   } else {
00387     logger->log_warn("WorldModelNetworkThread", "Opponent pose interface does not exist, ignoring");
00388   }
00389   __opponent_ifs.unlock();
00390 }
00391 
00392 
00393 void
00394 WorldModelNetworkThread::opponent_disapp_rcvd(const char *from_host, unsigned int uid)
00395 {
00396   __opponent_ifs.lock();
00397   std::map<std::string, std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> > >::iterator f;
00398   if ( ((f = __opponent_ifs.find(from_host)) != __opponent_ifs.end()) &&
00399        (f->second.find(uid) != f->second.end()) ) {
00400     blackboard->close(f->second[uid].second);
00401     f->second.erase(uid);
00402   }
00403   __opponent_ifs.unlock();
00404 }
00405 
00406 
00407 void
00408 WorldModelNetworkThread::gamestate_rcvd(const char *from_host,
00409                                         unsigned int game_state,
00410                                         fawkes::worldinfo_gamestate_team_t state_team,
00411                                         unsigned int score_cyan, unsigned int score_magenta,
00412                                         fawkes::worldinfo_gamestate_team_t our_team,
00413                                         fawkes::worldinfo_gamestate_goalcolor_t our_goal_color,
00414                                         fawkes::worldinfo_gamestate_half_t half)
00415 {
00416   logger->log_debug("WorldModelNetworkThread", "Received Gamestate %i from %s, state team %i, score %u:%u, our team: %i, our goal: %i, half: %i",
00417                     game_state, from_host, state_team, score_magenta, our_team, our_goal_color, half);
00418   switch (game_state) {
00419     case GS_FROZEN:
00420       __gamestate_if->set_game_state(GameStateInterface::GS_FROZEN);       break;
00421     case GS_PLAY:
00422       __gamestate_if->set_game_state(GameStateInterface::GS_PLAY);         break;
00423     case GS_KICK_OFF:
00424       __gamestate_if->set_game_state(GameStateInterface::GS_KICK_OFF);     break;
00425     case GS_DROP_BALL:
00426       __gamestate_if->set_game_state(GameStateInterface::GS_DROP_BALL);    break;
00427     case GS_PENALTY:
00428       __gamestate_if->set_game_state(GameStateInterface::GS_PENALTY);      break;
00429     case GS_CORNER_KICK:
00430       __gamestate_if->set_game_state(GameStateInterface::GS_CORNER_KICK);  break;
00431     case GS_THROW_IN:
00432       __gamestate_if->set_game_state(GameStateInterface::GS_THROW_IN);     break;
00433     case GS_FREE_KICK:
00434       __gamestate_if->set_game_state(GameStateInterface::GS_FREE_KICK);    break;
00435     case GS_GOAL_KICK:
00436       __gamestate_if->set_game_state(GameStateInterface::GS_GOAL_KICK);    break;
00437     case GS_HALF_TIME:
00438       __gamestate_if->set_game_state(GameStateInterface::GS_HALF_TIME);    break;
00439   }
00440 
00441   switch (state_team) {
00442     case TEAM_NONE:
00443       __gamestate_if->set_state_team(GameStateInterface::TEAM_NONE);       break;
00444     case TEAM_CYAN:
00445       __gamestate_if->set_state_team(GameStateInterface::TEAM_CYAN);       break;
00446     case TEAM_MAGENTA:
00447       __gamestate_if->set_state_team(GameStateInterface::TEAM_MAGENTA);    break;
00448     case TEAM_BOTH:
00449       __gamestate_if->set_state_team(GameStateInterface::TEAM_BOTH);       break;
00450   }
00451 
00452   switch (our_team) {
00453     case TEAM_NONE:
00454       __gamestate_if->set_our_team(GameStateInterface::TEAM_NONE);         break;
00455     case TEAM_CYAN:
00456       __gamestate_if->set_our_team(GameStateInterface::TEAM_CYAN);         break;
00457     case TEAM_MAGENTA:
00458       __gamestate_if->set_our_team(GameStateInterface::TEAM_MAGENTA);      break;
00459     case TEAM_BOTH:
00460       __gamestate_if->set_our_team(GameStateInterface::TEAM_BOTH);         break;
00461   }
00462 
00463   switch (our_goal_color) {
00464     case GOAL_BLUE:
00465       __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_BLUE);   break;
00466     case GOAL_YELLOW:
00467       __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_YELLOW); break;
00468   }
00469 
00470   switch (half) {
00471     case HALF_FIRST:
00472       __gamestate_if->set_half(GameStateInterface::HALF_FIRST);            break;
00473     case HALF_SECOND:
00474       __gamestate_if->set_half(GameStateInterface::HALF_SECOND);           break;
00475   }
00476 
00477   __gamestate_if->set_score_cyan(score_cyan);
00478   __gamestate_if->set_score_magenta(score_magenta);
00479 
00480   __gamestate_if->write();
00481 }
00482 
00483 
00484 void
00485 WorldModelNetworkThread::penalty_rcvd(const char *from_host,
00486                                       unsigned int player, unsigned int penalty,
00487                                       unsigned int seconds_remaining)
00488 {
00489   // TBD
00490 }
00491