void
WorldModelNetworkThread::global_ball_pos_rcvd(const char *from_host,
bool visible, int visibility_history,
float x, float y, float z,
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_WORLD |
ObjectPositionInterface::FLAG_HAS_Z_AS_ORI |
ObjectPositionInterface::FLAG_HAS_COVARIANCES );
iface->set_visible(visible);
iface->set_visibility_history(visibility_history);
iface->set_world_x(x);
iface->set_world_y(y);
iface->set_world_z(z);
iface->set_world_xyz_covariance(covariance);
iface->write();
__ball_ifs.unlock();
}
void
WorldModelNetworkThread::pose_rcvd(const char *from_host,
float x, float y, float theta,
float *covariance)
{
__pose_ifs.lock();
if (__pose_ifs.find(from_host) == __pose_ifs.end()) {
try {
std::string id = std::string("WI RoboPos ") + from_host;
__pose_ifs[from_host] = blackboard->open_for_writing<ObjectPositionInterface>(id.c_str());
} catch (Exception &e) {
logger->log_warn("WorldModelNetworkThread", "Failed to create ObjectPositionInterface "
"for pose of %s, exception follows", from_host);
logger->log_warn("WorldModelNetworkThread", e);
return;
}
}
// Pose is our aliveness indicator
__last_seen.lock();
__last_seen[from_host].stamp();
__last_seen.unlock();
ObjectPositionInterface *iface = __pose_ifs[from_host];
iface->set_world_x(x);
iface->set_world_y(y);
iface->set_world_z(theta);
iface->set_world_xyz_covariance(covariance);
iface->write();
__pose_ifs.unlock();
}
