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