Ejemplo n.º 1
0
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();
}
Ejemplo n.º 2
0
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();
}
Ejemplo n.º 3
0
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();
}
Ejemplo n.º 4
0
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();
}
Ejemplo n.º 5
0
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();
}