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