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(); }