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 #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