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