Exemplo n.º 1
0
u_instanceHandle
u_instanceHandleFix(
    u_instanceHandle _this,
    v_collection reader)
{
    u_instanceHandleTranslator translator;
    struct v_publicationInfo *data;
    v_topic topic;
    v_message message;
    v_public instance;

    translator.handle = _this;
    if (translator.lid.lifecycleId & HANDLE_GLOBAL_MASK) {
        /* Is a GID therefore fix handle by lookup. */
        while (v_objectKind(v_entity(reader)) == K_QUERY ||
               v_objectKind(v_entity(reader)) == K_DATAREADERQUERY ||
               v_objectKind(v_entity(reader)) == K_DATAVIEWQUERY) {
            /* If the entity derives from a query entity it can be cast to a v_query */
            reader = v_querySource(v_query(reader));
        }
        while (v_objectKind(v_entity(reader)) == K_DATAVIEW) {
            reader = v_collection(v_dataViewGetReader(v_dataView(reader)));
        }
        topic = v_dataReaderGetTopic(v_dataReader(reader));
        message = v_topicMessageNew(topic);
        data = (c_voidp)C_DISPLACE(message, v_topicDataOffset(topic));
        data->key = u_instanceHandleToGID(_this);
        instance = (v_public)v_dataReaderLookupInstance(v_dataReader(reader),
                                                        message);
        translator.handle = u_instanceHandleNew(instance);
        c_free(instance);
        c_free(topic);
        c_free(message);
    }
    return translator.handle;
}
Exemplo n.º 2
0
u_result
u_readerGetMatchedPublicationData (
    u_reader _this,
    u_instanceHandle publication_handle,
    v_statusAction action,
    c_voidp arg)
{
    v_dataReader reader;
    v_spliced spliced;
    v_kernel kernel;
    u_result result;
    c_iter participants;
    v_participant participant;

    result = U_RESULT_PRECONDITION_NOT_MET;
    if (_this != NULL) {
        result = u_entityReadClaim(u_entity(_this), (v_entity*)(&reader));

        if ((result == U_RESULT_OK) && (reader != NULL)) {
            kernel = v_objectKernel(reader);

            participants = v_resolveParticipants(kernel, V_SPLICED_NAME);
            assert(c_iterLength(participants) == 1);
            participant = v_participant(c_iterTakeFirst(participants));
            spliced = v_spliced(participant);
            c_free(participant);
            c_iterFree(participants);

            result = u_resultFromKernel(
                         v_splicedGetMatchedPublicationData(spliced, v_dataReader(reader), u_instanceHandleToGID(publication_handle), action, arg));
            u_entityRelease(u_entity(_this));
        }
    }
    return result;

}
Exemplo n.º 3
0
ROSIDL_TYPESUPPORT_OPENSPLICE_CPP_EXPORT_std_msgs
const char *
take__UInt16(
  void * untyped_topic_reader,
  bool ignore_local_publications,
  void * untyped_ros_message,
  bool * taken,
  void * sending_publication_handle)
{
  if (untyped_ros_message == 0) {
    return "invalid ros message pointer";
  };

  DDS::DataReader * topic_reader = static_cast<DDS::DataReader*>(untyped_topic_reader);

  std_msgs::msg::dds_::UInt16_DataReader * data_reader =
    std_msgs::msg::dds_::UInt16_DataReader::_narrow(topic_reader);

  std_msgs::msg::dds_::UInt16_Seq dds_messages;
  DDS::SampleInfoSeq sample_infos;
  DDS::ReturnCode_t status = data_reader->take(
    dds_messages,
    sample_infos,
    1,
    DDS::ANY_SAMPLE_STATE,
    DDS::ANY_VIEW_STATE,
    DDS::ANY_INSTANCE_STATE);

  const char * errs = nullptr;
  bool ignore_sample = false;

  switch (status) {
    case DDS::RETCODE_ERROR:
      errs = "std_msgs::msg::dds_::UInt16_DataReader.take: "
             "an internal error has occurred";
      goto finally;
    case DDS::RETCODE_ALREADY_DELETED:
      errs = "std_msgs::msg::dds_::UInt16_DataReader.take: "
             "this std_msgs::msg::dds_::UInt16_DataReader has already been deleted";
      goto finally;
    case DDS::RETCODE_OUT_OF_RESOURCES:
      errs = "std_msgs::msg::dds_::UInt16_DataReader.take: "
             "out of resources";
      goto finally;
    case DDS::RETCODE_NOT_ENABLED:
      errs = "std_msgs::msg::dds_::UInt16_DataReader.take: "
             "this std_msgs::msg::dds_::UInt16_DataReader is not enabled";
      goto finally;
    case DDS::RETCODE_PRECONDITION_NOT_MET:
      errs = "std_msgs::msg::dds_::UInt16_DataReader.take: "
             "a precondition is not met, one of: "
             "max_samples > maximum and max_samples != LENGTH_UNLIMITED, or "
             "the two sequences do not have matching parameters (length, maximum, release), or "
             "maximum > 0 and release is false.";
      goto finally;
    case DDS::RETCODE_NO_DATA:
      *taken = false;
      errs = nullptr;
      goto finally;
    case DDS::RETCODE_OK:
      break;
    default:
      errs = "std_msgs::msg::dds_::UInt16_DataReader.take: unknown return code";
      goto finally;
  }

  {
    DDS::SampleInfo & sample_info = sample_infos[0];
    if (!sample_info.valid_data) {
      // skip sample without data
      ignore_sample = true;
    } else {
      DDS::InstanceHandle_t sender_handle = sample_info.publication_handle;
      auto sender_gid = u_instanceHandleToGID(sender_handle);
      if (ignore_local_publications) {
        // compare the system id from the sender and this receiver
        // if they are equal the sample has been sent from this process and should be ignored
        DDS::InstanceHandle_t receiver_handle = topic_reader->get_instance_handle();
        auto receiver_gid = u_instanceHandleToGID(receiver_handle);
        ignore_sample = sender_gid.systemId == receiver_gid.systemId;
      }
      // This is nullptr when being used with plain rmw_take, so check first.
      if (sending_publication_handle) {
        *static_cast<DDS::InstanceHandle_t *>(sending_publication_handle) = sender_handle;
      }
    }
  }

  if (!ignore_sample) {
    __ros_msg_type & ros_message = *(__ros_msg_type *)untyped_ros_message;
    convert_dds_message_to_ros(dds_messages[0], ros_message);
    *taken = true;
  } else {
    *taken = false;
  }

finally:
  // Ensure the loan is returned.
  status = data_reader->return_loan(dds_messages, sample_infos);
  switch (status) {
    case DDS::RETCODE_ERROR:
      return "std_msgs::msg::dds_::UInt16_DataReader.return_loan: "
             "an internal error has occurred";
    case DDS::RETCODE_ALREADY_DELETED:
      return "std_msgs::msg::dds_::UInt16_DataReader.return_loan: "
             "this std_msgs::msg::dds_::UInt16_DataReader has already been deleted";
    case DDS::RETCODE_OUT_OF_RESOURCES:
      return "std_msgs::msg::dds_::UInt16_DataReader.return_loan: "
             "out of resources";
    case DDS::RETCODE_NOT_ENABLED:
      return "std_msgs::msg::dds_::UInt16_DataReader.return_loan: "
             "this std_msgs::msg::dds_::UInt16_DataReader is not enabled";
    case DDS::RETCODE_PRECONDITION_NOT_MET:
      return "std_msgs::msg::dds_::UInt16_DataReader.return_loan: "
             "a precondition is not met, one of: "
             "the data_values and info_seq do not belong to a single related pair, or "
             "the data_values and info_seq were not obtained from this "
             "std_msgs::msg::dds_::UInt16_DataReader";
    case DDS::RETCODE_OK:
      return nullptr;
    default:
      return "std_msgs::msg::dds_::UInt16_DataReader.return_loan failed with "
             "unknown return code";
  }

  return errs;
}