23 #include "net_thread.h"
25 #include <netcomm/worldinfo/transceiver.h>
26 #include <interfaces/ObjectPositionInterface.h>
27 #include <interfaces/GameStateInterface.h>
36 using namespace fawkes;
46 :
Thread(
"WorldModelNetworkThread",
Thread::OPMODE_CONTINUOUS)
48 __worldinfo_transceiver = NULL;
63 std::string multicast_addr;
65 std::string encryption_key;
66 std::string encryption_iv;
67 bool cfg_multicast_loopback;
73 __cfg_sleep_time_msec =
config->
get_uint(
"/worldinfo/sleep_time_msec");
74 __cfg_max_msgs_per_recv =
config->
get_uint(
"/worldinfo/max_msgs_per_recv");
75 __cfg_flush_time_sec =
config->
get_uint(
"/worldinfo/flush_time_sec");
76 cfg_multicast_loopback =
config->
get_bool(
"/worldinfo/multicast_loopback");
78 e.
append(
"Could not get required configuration data for worldmodel");
84 multicast_addr.c_str(), port,
85 encryption_key.c_str(), encryption_iv.c_str(),
89 __worldinfo_transceiver->
set_loop(cfg_multicast_loopback);
94 delete __worldinfo_transceiver;
106 i != __pose_ifs.end();
113 i != __ball_ifs.end();
120 i != __opponent_ifs.end();
122 for (UidTimeObjPosMap::iterator j = i->second.begin();
123 j != i->second.end();
130 delete __worldinfo_transceiver;
138 __worldinfo_transceiver->
recv(
false, __cfg_max_msgs_per_recv);
139 usleep( __cfg_sleep_time_msec * 1000 );
141 std::map<std::string, fawkes::Time>::iterator lsi = __last_seen.begin();
144 while (lsi != __last_seen.end()) {
145 if (now - &lsi->second > 3.0) {
146 logger->
log_info(
"WorldModelNetworkThread",
"Expiring host %s", lsi->first.c_str());
149 if (__pose_ifs.find(lsi->first) != __pose_ifs.end()) {
151 __pose_ifs.erase(lsi->first);
155 if (__ball_ifs.find(lsi->first) != __ball_ifs.end()) {
157 __ball_ifs.erase(lsi->first);
160 __opponent_ifs.
lock();
161 if (__opponent_ifs.find(lsi->first) != __opponent_ifs.end()) {
162 std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> >::iterator i;
163 for (i = __opponent_ifs[lsi->first].begin(); i != __opponent_ifs[lsi->first].end(); ++i) {
166 __opponent_ifs.erase(lsi->first);
169 std::map<std::string, fawkes::Time>::iterator tmp = lsi;
171 __last_seen.erase(tmp);
178 __opponent_ifs.
lock();
179 std::map<std::string, UidTimeObjPosMap>::iterator o = __opponent_ifs.begin();
180 while (o != __opponent_ifs.end()) {
181 UidTimeObjPosMap::iterator top = o->second.begin();
182 while (top != o->second.end()) {
183 if (now - &(top->second.first) > 3.0) {
184 logger->
log_info(
"WorldModelNetworkThread",
"Expiring Opponent %s:%u", o->first.c_str(), top->first);
186 UidTimeObjPosMap::iterator tmp = top;
188 o->second.erase(tmp);
193 if (o->second.empty()) {
194 std::map<std::string, UidTimeObjPosMap>::iterator tmp = o;
196 __opponent_ifs.erase(tmp);
212 return __worldinfo_transceiver;
218 float x,
float y,
float theta,
222 if (__pose_ifs.find(from_host) == __pose_ifs.end()) {
224 std::string
id = std::string(
"WI RoboPos ") + from_host;
227 logger->
log_warn(
"WorldModelNetworkThread",
"Failed to create ObjectPositionInterface "
228 "for pose of %s, exception follows", from_host);
236 __last_seen[from_host].stamp();
251 float vel_y,
float vel_theta,
float *covariance)
259 bool visible,
int visibility_history,
260 float dist,
float bearing,
float slope,
264 if (__ball_ifs.find(from_host) == __ball_ifs.end()) {
266 std::string
id = std::string(
"WI BPos ") + from_host;
269 logger->
log_warn(
"WorldModelNetworkThread",
"Failed to create ObjectPositionInterface "
270 "for ball pos of %s, exception follows", from_host);
278 ObjectPositionInterface::TYPE_BALL |
279 ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR |
280 ObjectPositionInterface::FLAG_HAS_COVARIANCES );
294 bool visible,
int visibility_history,
295 float x,
float y,
float z,
299 if (__ball_ifs.find(from_host) == __ball_ifs.end()) {
301 std::string
id = std::string(
"WI BPos ") + from_host;
304 logger->
log_warn(
"WorldModelNetworkThread",
"Failed to create ObjectPositionInterface "
305 "for ball pos of %s, exception follows", from_host);
313 ObjectPositionInterface::TYPE_BALL |
314 ObjectPositionInterface::FLAG_HAS_WORLD |
315 ObjectPositionInterface::FLAG_HAS_Z_AS_ORI |
316 ObjectPositionInterface::FLAG_HAS_COVARIANCES );
330 float vel_x,
float vel_y,
float vel_z,
339 float vel_x,
float vel_y,
float vel_z,
349 float distance,
float bearing,
352 __opponent_ifs.
lock();
353 std::map<std::string, std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> > >::iterator f;
355 bool iface_exists =
true;
356 if ( ((f = __opponent_ifs.find(from_host)) == __opponent_ifs.end()) ||
357 (f->second.find(uid) == f->second.end()) ) {
360 if (asprintf(&tmp,
"WI Opp %u %s", ++__opponent_id, from_host) != -1) {
362 std::string
id = tmp;
364 logger->
log_debug(
"WorldModelNetworkThread",
"Opening new interface for %s:%u", from_host, uid);
367 logger->
log_warn(
"WorldModelNetworkThread",
"Failed to create ObjectPositionInterface "
368 "for opponent %s:%u, exception follows", from_host, uid);
370 iface_exists =
false;
373 logger->
log_error(
"WorldModelNetworkThread",
"Could not create interface ID string, out of memory during asprintf().");
374 iface_exists =
false;
379 logger->
log_debug(
"WorldModelNetworkThread",
"Setting opponent %s:%u", from_host, uid);
386 __opponent_ifs[from_host][uid].first.stamp();
388 logger->
log_warn(
"WorldModelNetworkThread",
"Opponent pose interface does not exist, ignoring");
397 __opponent_ifs.
lock();
398 std::map<std::string, std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> > >::iterator f;
399 if ( ((f = __opponent_ifs.find(from_host)) != __opponent_ifs.end()) &&
400 (f->second.find(uid) != f->second.end()) ) {
402 f->second.erase(uid);
410 unsigned int game_state,
412 unsigned int score_cyan,
unsigned int score_magenta,
417 logger->
log_debug(
"WorldModelNetworkThread",
"Received Gamestate %i from %s, state team %i, score %u:%u, our team: %i, our goal: %i, half: %i",
418 game_state, from_host, state_team, score_magenta, our_team, our_goal_color, half);
419 switch (game_state) {
421 __gamestate_if->
set_game_state(GameStateInterface::GS_FROZEN);
break;
423 __gamestate_if->
set_game_state(GameStateInterface::GS_PLAY);
break;
425 __gamestate_if->
set_game_state(GameStateInterface::GS_KICK_OFF);
break;
427 __gamestate_if->
set_game_state(GameStateInterface::GS_DROP_BALL);
break;
429 __gamestate_if->
set_game_state(GameStateInterface::GS_PENALTY);
break;
431 __gamestate_if->
set_game_state(GameStateInterface::GS_CORNER_KICK);
break;
433 __gamestate_if->
set_game_state(GameStateInterface::GS_THROW_IN);
break;
435 __gamestate_if->
set_game_state(GameStateInterface::GS_FREE_KICK);
break;
437 __gamestate_if->
set_game_state(GameStateInterface::GS_GOAL_KICK);
break;
439 __gamestate_if->
set_game_state(GameStateInterface::GS_HALF_TIME);
break;
442 switch (state_team) {
444 __gamestate_if->
set_state_team(GameStateInterface::TEAM_NONE);
break;
446 __gamestate_if->
set_state_team(GameStateInterface::TEAM_CYAN);
break;
448 __gamestate_if->
set_state_team(GameStateInterface::TEAM_MAGENTA);
break;
450 __gamestate_if->
set_state_team(GameStateInterface::TEAM_BOTH);
break;
455 __gamestate_if->
set_our_team(GameStateInterface::TEAM_NONE);
break;
457 __gamestate_if->
set_our_team(GameStateInterface::TEAM_CYAN);
break;
459 __gamestate_if->
set_our_team(GameStateInterface::TEAM_MAGENTA);
break;
461 __gamestate_if->
set_our_team(GameStateInterface::TEAM_BOTH);
break;
464 switch (our_goal_color) {
473 __gamestate_if->
set_half(GameStateInterface::HALF_FIRST);
break;
475 __gamestate_if->
set_half(GameStateInterface::HALF_SECOND);
break;
481 __gamestate_if->
write();
487 unsigned int player,
unsigned int penalty,
488 unsigned int seconds_remaining)