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