void
WorldModelNetworkThread::ball_pos_rcvd(const char *from_host,
bool visible, int visibility_history,
float dist, float bearing, float slope,
float *covariance)
{
__ball_ifs.lock();
if (__ball_ifs.find(from_host) == __ball_ifs.end()) {
try {
std::string id = std::string("WI BPos ") + from_host;
__ball_ifs[from_host] = blackboard->open_for_writing<ObjectPositionInterface>(id.c_str());
} catch (Exception &e) {
logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
"for ball pos of %s, exception follows", from_host);
logger->log_warn("WorldModelNetworkThread", e);
return;
}
}
ObjectPositionInterface *iface = __ball_ifs[from_host];
iface->set_flags( iface->flags() |
ObjectPositionInterface::TYPE_BALL |
ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR |
ObjectPositionInterface::FLAG_HAS_COVARIANCES );
iface->set_visible(visible);
iface->set_visibility_history(visibility_history);
iface->set_distance(dist);
iface->set_bearing(bearing);
iface->set_slope(slope);
iface->set_dbs_covariance(covariance);
iface->write();
__ball_ifs.unlock();
}
void
WorldModelNetworkThread::opponent_pose_rcvd(const char *from_host,
unsigned int uid,
float distance, float bearing,
float *covariance)
{
__opponent_ifs.lock();
std::map<std::string, std::map<unsigned int, std::pair<Time, ObjectPositionInterface *> > >::iterator f;
bool iface_exists = true;
if ( ((f = __opponent_ifs.find(from_host)) == __opponent_ifs.end()) ||
(f->second.find(uid) == f->second.end()) ) {
char *tmp;
if (asprintf(&tmp, "WI Opp %u %s", ++__opponent_id, from_host) != -1) {
try {
std::string id = tmp;
free(tmp);
logger->log_debug("WorldModelNetworkThread", "Opening new interface for %s:%u", from_host, uid);
__opponent_ifs[from_host][uid] = make_pair(Time(), blackboard->open_for_writing<ObjectPositionInterface>(id.c_str()));
} catch (Exception &e) {
logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
"for opponent %s:%u, exception follows", from_host, uid);
logger->log_warn("WorldModelNetworkThread", e);
iface_exists = false;
}
} else {
logger->log_error("WorldModelNetworkThread", "Could not create interface ID string, out of memory during asprintf().");
iface_exists = false;
}
}
if (iface_exists) {
logger->log_debug("WorldModelNetworkThread", "Setting opponent %s:%u", from_host, uid);
ObjectPositionInterface *iface = __opponent_ifs[from_host][uid].second;
iface->set_distance(distance);
iface->set_bearing(bearing);
iface->set_dbs_covariance(covariance);
iface->write();
__opponent_ifs[from_host][uid].first.stamp();
} else {
logger->log_warn("WorldModelNetworkThread", "Opponent pose interface does not exist, ignoring");
}
__opponent_ifs.unlock();
}
