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::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::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();
}
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();
}
void
WorldModelObjPosAverageFuser::fuse()
{
MutexLocker lock(__input_ifs.mutex());
unsigned int flags = 0;
unsigned int base_flags = 0;
unsigned int world_num_inputs=0, extent_num_inputs=0, euler_num_inputs = 0,
worldvel_num_inputs = 0, relcart_num_inputs = 0, relpolar_num_inputs = 0;
float roll = 0, pitch = 0, yaw = 0, distance = 0, bearing = 0, slope = 0,
world_x = 0, world_y = 0, world_z = 0,
relative_x = 0, relative_y = 0, relative_z = 0,
extent_x = 0, extent_y = 0, extent_z = 0,
world_x_velocity = 0, world_y_velocity = 0, world_z_velocity = 0,
relative_x_velocity = 0, relative_y_velocity = 0, relative_z_velocity = 0;
bool valid = true, visible = true;
int vishistory_min = 0, vishistory_max = 0;
unsigned int object_type = 0;
bool object_type_warned = false;
bool flags_read = false;
bool have_world = false, have_relative = false;
for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
ObjectPositionInterface *iface = *__iii;
if (iface->has_writer()) {
iface->read();
if (iface->is_valid()) {
if ( (object_type != 0) && (iface->object_type() != object_type) &&
! object_type_warned) {
__logger->log_warn("WMObjPosAvgFus", "Object types of input interfaces "
"for %s disagree, %s has %u, expected was %u",
__to_id.c_str(), iface->uid(), iface->object_type(),
object_type);
object_type_warned = true;
} else {
object_type = iface->object_type();
}
if (flags_read) {
unsigned int iflags = iface->flags()
& (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_WORLD)
& (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN)
& (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR);
if (iflags != base_flags) {
__logger->log_warn("WMObjPosAvgFus", "Interface flags for %s "
"disagree. Exected %x, got %x", base_flags, iflags);
}
} else {
base_flags = iface->flags()
& (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_WORLD)
& (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN)
& (0xFFFFFFFF^ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR);
flags_read = true;
}
if (iface->is_visible()) {
if (iface->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
have_world = true;
flags |= ObjectPositionInterface::FLAG_HAS_WORLD;
world_x += iface->world_x();
world_y += iface->world_y();
world_z += iface->world_z();
world_num_inputs += 1;
if (iface->flags() & ObjectPositionInterface::FLAG_HAS_EULER_ANGLES) {
roll += iface->roll();
pitch += iface->pitch();
yaw += iface->yaw();
flags |= ObjectPositionInterface::FLAG_HAS_EULER_ANGLES;
euler_num_inputs += 1;
}
if (iface->flags() & ObjectPositionInterface::FLAG_HAS_WORLD_VELOCITY) {
world_x_velocity += iface->world_x_velocity();
world_y_velocity += iface->world_y_velocity();
world_z_velocity += iface->world_z_velocity();
flags |= ObjectPositionInterface::FLAG_HAS_WORLD_VELOCITY;
worldvel_num_inputs += 1;
}
}
if (iface->flags() & ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN) {
have_relative = true;
flags |= ObjectPositionInterface::FLAG_HAS_RELATIVE_CARTESIAN;
relative_x += iface->relative_x();
relative_y += iface->relative_y();
relative_z += iface->relative_z();
relative_x_velocity += iface->relative_x_velocity();
relative_y_velocity += iface->relative_y_velocity();
relative_z_velocity += iface->relative_z_velocity();
relcart_num_inputs += 1;
}
if (iface->flags() & ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR) {
have_relative = true;
flags |= ObjectPositionInterface::FLAG_HAS_RELATIVE_POLAR;
distance += iface->distance();
bearing += iface->bearing();
slope += iface->slope();
relpolar_num_inputs += 1;
}
if (iface->flags() & ObjectPositionInterface::FLAG_HAS_EXTENT) {
extent_x += iface->extent_x();
extent_y += iface->extent_y();
extent_z += iface->extent_z();
flags |= ObjectPositionInterface::FLAG_HAS_EXTENT;
extent_num_inputs += 1;
}
if (iface->visibility_history() > vishistory_max) {
vishistory_max = iface->visibility_history();
}
} else {
if (iface->visibility_history() < vishistory_min) {
vishistory_min = iface->visibility_history();
}
}
}
}
}
if ( world_num_inputs > 0 ) {
__output_if->set_world_x(world_x / (float)world_num_inputs);
__output_if->set_world_y(world_y / (float)world_num_inputs);
__output_if->set_world_z(world_z / (float)world_num_inputs);
}
if ( euler_num_inputs > 0 ) {
__output_if->set_roll(roll / (float)euler_num_inputs);
__output_if->set_pitch(pitch / (float)euler_num_inputs);
__output_if->set_yaw(yaw / (float)euler_num_inputs);
}
if ( worldvel_num_inputs > 0) {
__output_if->set_world_x_velocity(world_x_velocity / (float)worldvel_num_inputs);
__output_if->set_world_y_velocity(world_y_velocity / (float)worldvel_num_inputs);
__output_if->set_world_z_velocity(world_z_velocity / (float)worldvel_num_inputs);
}
if ( extent_num_inputs > 0 ) {
__output_if->set_extent_x(extent_x / (float)extent_num_inputs);
__output_if->set_extent_y(extent_y / (float)extent_num_inputs);
__output_if->set_extent_z(extent_z / (float)extent_num_inputs);
}
if ( relcart_num_inputs > 0) {
__output_if->set_relative_x(relative_x / (float)relcart_num_inputs);
__output_if->set_relative_y(relative_y / (float)relcart_num_inputs);
__output_if->set_relative_z(relative_z / (float)relcart_num_inputs);
__output_if->set_relative_x_velocity(relative_x_velocity / (float)relcart_num_inputs);
__output_if->set_relative_y_velocity(relative_y_velocity / (float)relcart_num_inputs);
__output_if->set_relative_z_velocity(relative_z_velocity / (float)relcart_num_inputs);
}
if ( relpolar_num_inputs > 0) {
__output_if->set_distance(distance / (float)relpolar_num_inputs);
__output_if->set_bearing(bearing / (float)relpolar_num_inputs);
__output_if->set_slope(slope / (float)relpolar_num_inputs);
}
visible = have_world || have_relative;
__output_if->set_flags(flags);
__output_if->set_valid(valid);
__output_if->set_visible(visible);
__output_if->set_visibility_history(visible ? vishistory_max : vishistory_min);
__output_if->write();
}
