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)
virtual void ball_pos_rcvd(const char *from_host, bool visible, int visibility_history, float dist, float bearing, float slope, float *covariance)
Ball position information received.
virtual void init()
Initialize the thread.
void set_game_state(const uint32_t new_game_state)
Set game_state value.
Class to send and receive world information.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void set_score_cyan(const uint32_t new_score_cyan)
Set score_cyan value.
ObjectPositionInterface Fawkes BlackBoard Interface.
Referee drops ball, both teams can wrestle for the ball.
void set_our_goal_color(const if_gamestate_goalcolor_t new_our_goal_color)
Set our_goal_color value.
void unlock() const
Unlock list.
No team, not team-specific.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_world_z(const float new_world_z)
Set world_z value.
void set_state_team(const if_gamestate_team_t new_state_team)
Set state_team value.
void set_flags(const uint32_t new_flags)
Set flags value.
uint32_t flags() const
Get flags value.
A class for handling time.
void set_distance(const float new_distance)
Set distance value.
Thread class encapsulation of pthreads.
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
virtual void opponent_pose_rcvd(const char *from_host, unsigned int uid, float distance, float bearing, float *covariance)
Opponent information received.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
virtual void global_ball_pos_rcvd(const char *from_host, bool visible, int visibility_history, float x, float y, float z, float *covariance)
Global ball position information received.
worldinfo_gamestate_half_t
Game time half.
fawkes::WorldInfoTransceiver * get_transceiver()
Access the WI transceiver.
virtual void loop()
Code to execute in the thread.
virtual void global_ball_velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_z, float *covariance)
Ball velocity information received.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
void lock() const
Lock list.
void set_loop(bool loop)
Set loopback of sent packets.
void set_dbs_covariance(unsigned int index, const float new_dbs_covariance)
Set dbs_covariance value at given index.
virtual void gamestate_rcvd(const char *from_host, unsigned int game_state, fawkes::worldinfo_gamestate_team_t state_team, unsigned int score_cyan, unsigned int score_magenta, fawkes::worldinfo_gamestate_team_t our_team, fawkes::worldinfo_gamestate_goalcolor_t our_goal_color, fawkes::worldinfo_gamestate_half_t half)
Gamestate information received.
NetworkNameResolver * nnresolver
Network name resolver to lookup IP addresses of hostnames and vice versa.
void recv(bool block=false, unsigned int max_num_msgs=0)
Receive information.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void set_visible(const bool new_visible)
Set visible value.
void set_bearing(const float new_bearing)
Set bearing value.
void flush_sequence_numbers(unsigned int sec)
Flush sequence numbers conditionally.
void print_trace()
Prints trace to stderr.
void set_slope(const float new_slope)
Set slope value.
virtual void pose_rcvd(const char *from_host, float x, float y, float theta, float *covariance)
Pose information received.
void set_world_x(const float new_world_x)
Set world_x value.
WorldModelNetworkThread()
Constructor.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
worldinfo_gamestate_team_t
Team.
virtual void finalize()
Finalize the thread.
virtual void ball_velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_z, float *covariance)
Ball velocity information received.
GameStateInterface Fawkes BlackBoard Interface.
void add_handler(WorldInfoHandler *h)
Add a handler for world information.
void set_world_xyz_covariance(unsigned int index, const float new_world_xyz_covariance)
Set world_xyz_covariance value at given index.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
void set_score_magenta(const uint32_t new_score_magenta)
Set score_magenta value.
virtual void opponent_disapp_rcvd(const char *from_host, unsigned int uid)
Opponent disappeared.
Configuration * config
This is the Configuration member used to access the configuration.
worldinfo_gamestate_goalcolor_t
Goal color.
virtual void penalty_rcvd(const char *from_host, unsigned int player, unsigned int penalty, unsigned int seconds_remaining)
Penalty info received.
void append(const char *format,...)
Append messages to the message list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
void set_half(const if_gamestate_half_t new_half)
Set half value.
virtual ~WorldModelNetworkThread()
Destructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
void set_our_team(const if_gamestate_team_t new_our_team)
Set our_team value.
virtual void velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_theta, float *covariance)
Robot velocity information received.
void set_world_y(const float new_world_y)
Set world_y value.
virtual void close(Interface *interface)=0
Close interface.