00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00038
00039
00040
00041
00042
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
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
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
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
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
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
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
00206
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
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
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
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
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
00490 }
00491