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