bool
get_datawriter_qos(
DDSDomainParticipant * participant,
const rmw_qos_profile_t & qos_profile,
DDS_DataWriterQos & datawriter_qos)
{
DDS_ReturnCode_t status = participant->get_default_datawriter_qos(datawriter_qos);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to get default datawriter qos");
return false;
}
status = DDSPropertyQosPolicyHelper::add_property(
datawriter_qos.property,
"dds.data_writer.history.memory_manager.fast_pool.pool_buffer_max_size",
"4096",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return false;
}
if (!set_entity_qos_from_profile(qos_profile, datawriter_qos)) {
return false;
}
// TODO(wjwwood): conditionally use the async publish mode using a heuristic:
// https://github.com/ros2/rmw_connext/issues/190
datawriter_qos.publish_mode.kind = DDS::ASYNCHRONOUS_PUBLISH_MODE_QOS;
return true;
}
rmw_ret_t
__rmw_node_assert_liveliness(
const char * identifier,
const rmw_node_t * node)
{
RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
node,
node->implementation_identifier,
identifier,
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
auto node_info = static_cast<CustomParticipantInfo *>(node->data);
if (nullptr == node_info) {
RMW_SET_ERROR_MSG("node info handle is null");
return RMW_RET_ERROR;
}
if (nullptr == node_info->participant) {
RMW_SET_ERROR_MSG("node internal participant is invalid");
return RMW_RET_ERROR;
}
// node_info->participant->assert_liveliness();
RMW_SET_ERROR_MSG("assert_liveliness() of node is currently not supported");
return RMW_RET_UNSUPPORTED;
}
rmw_ret_t
rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid)
{
if (!publisher) {
RMW_SET_ERROR_MSG("publisher is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
publisher handle,
publisher->implementation_identifier,
opensplice_cpp_identifier,
return RMW_RET_ERROR)
if (!gid) {
RMW_SET_ERROR_MSG("gid is null");
return RMW_RET_ERROR;
}
const OpenSpliceStaticPublisherInfo * publisher_info =
static_cast<const OpenSpliceStaticPublisherInfo *>(publisher->data);
if (!publisher_info) {
RMW_SET_ERROR_MSG("publisher info handle is null");
return RMW_RET_ERROR;
}
*gid = publisher_info->publisher_gid;
return RMW_RET_OK;
}
rmw_guard_condition_t *
create_guard_condition(const char * implementation_identifier)
{
rmw_guard_condition_t * guard_condition = rmw_guard_condition_allocate();
if (!guard_condition) {
RMW_SET_ERROR_MSG("failed to allocate guard condition");
return NULL;
}
// Allocate memory for the DDSGuardCondition object.
DDSGuardCondition * dds_guard_condition = nullptr;
void * buf = rmw_allocate(sizeof(DDSGuardCondition));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
// Use a placement new to construct the DDSGuardCondition in the preallocated buffer.
RMW_TRY_PLACEMENT_NEW(dds_guard_condition, buf, goto fail, DDSGuardCondition)
buf = nullptr; // Only free the dds_guard_condition pointer; don't need the buf pointer anymore.
guard_condition->implementation_identifier = implementation_identifier;
guard_condition->data = dds_guard_condition;
return guard_condition;
fail:
if (guard_condition) {
rmw_guard_condition_free(guard_condition);
}
if (buf) {
rmw_free(buf);
}
return NULL;
}
rmw_waitset_t *
create_waitset(const char * implementation_identifier, size_t max_conditions)
{
rmw_waitset_t * waitset = rmw_waitset_allocate();
ConnextWaitSetInfo * waitset_info = nullptr;
// From here onward, error results in unrolling in the goto fail block.
if (!waitset) {
RMW_SET_ERROR_MSG("failed to allocate waitset");
goto fail;
}
waitset->implementation_identifier = implementation_identifier;
waitset->data = rmw_allocate(sizeof(ConnextWaitSetInfo));
waitset_info = static_cast<ConnextWaitSetInfo *>(waitset->data);
if (!waitset_info) {
RMW_SET_ERROR_MSG("failed to allocate waitset");
goto fail;
}
waitset_info->waitset = static_cast<DDSWaitSet *>(rmw_allocate(sizeof(DDSWaitSet)));
if (!waitset_info->waitset) {
RMW_SET_ERROR_MSG("failed to allocate waitset");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
waitset_info->waitset, waitset_info->waitset, goto fail, DDSWaitSet)
// Now allocate storage for the ConditionSeq objects
waitset_info->active_conditions =
static_cast<DDSConditionSeq *>(rmw_allocate(sizeof(DDSConditionSeq)));
if (!waitset_info->active_conditions) {
RMW_SET_ERROR_MSG("failed to allocate active_conditions sequence");
goto fail;
}
waitset_info->attached_conditions =
static_cast<DDSConditionSeq *>(rmw_allocate(sizeof(DDSConditionSeq)));
if (!waitset_info->attached_conditions) {
RMW_SET_ERROR_MSG("failed to allocate attached_conditions sequence");
goto fail;
}
// If max_conditions is greater than zero, re-allocate both ConditionSeqs to max_conditions
if (max_conditions > 0) {
RMW_TRY_PLACEMENT_NEW(
waitset_info->active_conditions, waitset_info->active_conditions, goto fail,
DDSConditionSeq, static_cast<DDS_Long>(max_conditions))
RMW_TRY_PLACEMENT_NEW(
waitset_info->attached_conditions, waitset_info->attached_conditions, goto fail,
DDSConditionSeq,
static_cast<DDS_Long>(max_conditions))
} else {
rmw_ret_t
rmw_destroy_client(rmw_client_t * client)
{
if (!client) {
RMW_SET_ERROR_MSG("client handle is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
client handle,
client->implementation_identifier, opensplice_cpp_identifier,
return RMW_RET_ERROR)
OpenSpliceStaticClientInfo * client_info =
static_cast<OpenSpliceStaticClientInfo *>(client->data);
auto result = RMW_RET_OK;
if (client_info) {
auto response_datareader = client_info->response_datareader_;
if (response_datareader) {
auto read_condition = client_info->read_condition_;
if (read_condition) {
if (response_datareader->delete_readcondition(read_condition) != DDS::RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to delete readcondition");
result = RMW_RET_ERROR;
}
client_info->read_condition_ = nullptr;
}
}
} else {
RMW_SET_ERROR_MSG("client_info handle is null");
return RMW_RET_ERROR;
}
const service_type_support_callbacks_t * callbacks =
static_cast<const service_type_support_callbacks_t *>(client_info->callbacks_);
if (!callbacks) {
RMW_SET_ERROR_MSG("callbacks handle is null");
return RMW_RET_ERROR;
}
const char * error_string = callbacks->destroy_requester(client_info->requester_, &rmw_free);
if (error_string) {
RMW_SET_ERROR_MSG((std::string("failed to destroy requester: ") + error_string).c_str());
return RMW_RET_ERROR;
}
if (client->service_name) {
rmw_free(const_cast<char *>(client->service_name));
}
rmw_free(client_info);
rmw_client_free(client);
return result;
}
rmw_ret_t check_attach_condition_error(DDS::ReturnCode_t retcode)
{
if (retcode == DDS_RETCODE_OK) {
return RMW_RET_OK;
}
if (retcode == DDS_RETCODE_OUT_OF_RESOURCES) {
RMW_SET_ERROR_MSG("failed to attach condition to waitset: out of resources");
} else if (retcode == DDS_RETCODE_BAD_PARAMETER) {
RMW_SET_ERROR_MSG("failed to attach condition to waitset: condition pointer was invalid");
} else {
RMW_SET_ERROR_MSG("failed to attach condition to waitset");
}
return RMW_RET_ERROR;
}
rmw_ret_t
rmw_deserialize(
const rmw_serialized_message_t * serialized_message,
const rosidl_message_type_support_t * type_support,
void * ros_message)
{
const rosidl_message_type_support_t * ts = get_message_typesupport_handle(
type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_C);
if (!ts) {
ts = get_message_typesupport_handle(
type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP);
if (!ts) {
RMW_SET_ERROR_MSG("type support not from this implementation");
return RMW_RET_ERROR;
}
}
auto callbacks = static_cast<const message_type_support_callbacks_t *>(ts->data);
auto tss = new MessageTypeSupport_cpp(callbacks);
eprosima::fastcdr::FastBuffer buffer(
reinterpret_cast<char *>(serialized_message->buffer), serialized_message->buffer_length);
eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
eprosima::fastcdr::Cdr::DDS_CDR);
auto ret = tss->deserializeROSmessage(deser, ros_message);
delete tss;
return ret == true ? RMW_RET_OK : RMW_RET_ERROR;
}
rmw_ret_t
rmw_take_serialized_message_with_info(
const rmw_subscription_t * subscription,
rmw_serialized_message_t * serialized_message,
bool * taken,
rmw_message_info_t * message_info,
rmw_subscription_allocation_t * allocation)
{
if (!message_info) {
RMW_SET_ERROR_MSG("message info is null");
return RMW_RET_ERROR;
}
DDS::InstanceHandle_t sending_publication_handle;
auto ret =
_take_serialized_message(subscription, serialized_message, taken,
&sending_publication_handle, allocation);
if (ret != RMW_RET_OK) {
// Error string is already set.
return RMW_RET_ERROR;
}
rmw_gid_t * sender_gid = &message_info->publisher_gid;
sender_gid->implementation_identifier = rti_connext_identifier;
memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE);
auto detail = reinterpret_cast<ConnextPublisherGID *>(sender_gid->data);
detail->publication_handle = sending_publication_handle;
return RMW_RET_OK;
}
rmw_ret_t
rmw_get_serialized_message_size(
const rosidl_message_type_support_t * /*type_support*/,
const rosidl_message_bounds_t * /*message_bounds*/,
size_t * /*size*/)
{
RMW_SET_ERROR_MSG("unimplemented");
return RMW_RET_ERROR;
}
const void * get_response_ptr(const void * untyped_service_members)
{
auto service_members = static_cast<const ServiceType *>(untyped_service_members);
if (!service_members) {
RMW_SET_ERROR_MSG("service members handle is null");
return NULL;
}
return service_members->response_members_;
}
const rmw_guard_condition_t *
__rmw_node_get_graph_guard_condition(const rmw_node_t * node)
{
auto impl = static_cast<CustomParticipantInfo *>(node->data);
if (!impl) {
RMW_SET_ERROR_MSG("node impl is null");
return nullptr;
}
return impl->graph_guard_condition;
}
rmw_ret_t
init()
{
DDSDomainParticipantFactory * dpf_ = DDSDomainParticipantFactory::get_instance();
if (!dpf_) {
RMW_SET_ERROR_MSG("failed to get participant factory");
return RMW_RET_ERROR;
}
return RMW_RET_OK;
}
bool
is_valid_qos(
const rmw_qos_profile_t & qos_policies)
{
if (!is_time_default(qos_policies.deadline)) {
RMW_SET_ERROR_MSG("Deadline unsupported for fastrtps");
return false;
}
if (!is_time_default(qos_policies.lifespan)) {
RMW_SET_ERROR_MSG("Lifespan unsupported for fastrtps");
return false;
}
if (qos_policies.liveliness == RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE ||
qos_policies.liveliness == RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC)
{
RMW_SET_ERROR_MSG("Liveliness unsupported for fastrtps");
return false;
}
return true;
}
rmw_ret_t
__rmw_send_response(
const char * identifier,
const rmw_service_t * service,
rmw_request_id_t * request_header,
void * ros_response)
{
assert(service);
assert(request_header);
assert(ros_response);
rmw_ret_t returnedValue = RMW_RET_ERROR;
if (service->implementation_identifier != identifier) {
RMW_SET_ERROR_MSG("service handle not from this implementation");
return RMW_RET_ERROR;
}
auto info = static_cast<CustomServiceInfo *>(service->data);
assert(info);
eprosima::fastrtps::rtps::WriteParams wparams;
memcpy(&wparams.related_sample_identity().writer_guid(), request_header->writer_guid,
sizeof(eprosima::fastrtps::rtps::GUID_t));
wparams.related_sample_identity().sequence_number().high =
(int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32);
wparams.related_sample_identity().sequence_number().low =
(int32_t)(request_header->sequence_number & 0xFFFFFFFF);
rmw_fastrtps_shared_cpp::SerializedData data;
data.is_cdr_buffer = false;
data.data = const_cast<void *>(ros_response);
if (info->response_publisher_->write(&data, wparams)) {
returnedValue = RMW_RET_OK;
} else {
RMW_SET_ERROR_MSG("cannot publish data");
}
return returnedValue;
}
rmw_ret_t
rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result)
{
if (!gid1) {
RMW_SET_ERROR_MSG("gid1 is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
gid1,
gid1->implementation_identifier,
rti_connext_identifier,
return RMW_RET_ERROR)
if (!gid2) {
RMW_SET_ERROR_MSG("gid2 is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
gid2,
gid2->implementation_identifier,
rti_connext_identifier,
return RMW_RET_ERROR)
if (!result) {
RMW_SET_ERROR_MSG("result is null");
return RMW_RET_ERROR;
}
auto detail1 = reinterpret_cast<const ConnextPublisherGID *>(gid1->data);
if (!detail1) {
RMW_SET_ERROR_MSG("gid1 is invalid");
return RMW_RET_ERROR;
}
auto detail2 = reinterpret_cast<const ConnextPublisherGID *>(gid2->data);
if (!detail2) {
RMW_SET_ERROR_MSG("gid2 is invalid");
return RMW_RET_ERROR;
}
auto matches =
DDS_InstanceHandle_equals(&detail1->publication_handle, &detail2->publication_handle);
*result = (matches == DDS::BOOLEAN_TRUE);
return RMW_RET_OK;
}
rmw_ret_t
_take_serialized_message(
const rmw_subscription_t * subscription,
rmw_serialized_message_t * serialized_message,
bool * taken,
DDS::InstanceHandle_t * sending_publication_handle,
rmw_subscription_allocation_t * allocation)
{
if (!subscription) {
RMW_SET_ERROR_MSG("subscription handle is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
subscription handle,
subscription->implementation_identifier, rti_connext_identifier,
return RMW_RET_ERROR)
if (!serialized_message) {
RMW_SET_ERROR_MSG("ros message handle is null");
return RMW_RET_ERROR;
}
if (!taken) {
RMW_SET_ERROR_MSG("taken handle is null");
return RMW_RET_ERROR;
}
ConnextStaticSubscriberInfo * subscriber_info =
static_cast<ConnextStaticSubscriberInfo *>(subscription->data);
if (!subscriber_info) {
RMW_SET_ERROR_MSG("subscriber info handle is null");
return RMW_RET_ERROR;
}
DDS::DataReader * topic_reader = subscriber_info->topic_reader_;
if (!topic_reader) {
RMW_SET_ERROR_MSG("topic reader handle is null");
return RMW_RET_ERROR;
}
const message_type_support_callbacks_t * callbacks = subscriber_info->callbacks_;
if (!callbacks) {
RMW_SET_ERROR_MSG("callbacks handle is null");
return RMW_RET_ERROR;
}
// fetch the incoming message as cdr stream
if (!take(
topic_reader, subscriber_info->ignore_local_publications, serialized_message, taken,
sending_publication_handle, allocation))
{
RMW_SET_ERROR_MSG("error occured while taking message");
return RMW_RET_ERROR;
}
return RMW_RET_OK;
}
rmw_ret_t
rmw_serialize(
const void * ros_message,
const rosidl_message_type_support_t * type_support,
rmw_serialized_message_t * serialized_message)
{
const rosidl_message_type_support_t * ts = get_message_typesupport_handle(
type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_C);
if (!ts) {
ts = get_message_typesupport_handle(
type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP);
if (!ts) {
RMW_SET_ERROR_MSG("type support not from this implementation");
return RMW_RET_ERROR;
}
}
auto callbacks = static_cast<const message_type_support_callbacks_t *>(ts->data);
auto tss = new MessageTypeSupport_cpp(callbacks);
auto data_length = tss->getEstimatedSerializedSize(ros_message);
if (serialized_message->buffer_capacity < data_length) {
if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) {
RMW_SET_ERROR_MSG("unable to dynamically resize serialized message");
return RMW_RET_ERROR;
}
}
eprosima::fastcdr::FastBuffer buffer(
reinterpret_cast<char *>(serialized_message->buffer), data_length);
eprosima::fastcdr::Cdr ser(
buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
auto ret = tss->serializeROSmessage(ros_message, ser);
serialized_message->buffer_length = data_length;
serialized_message->buffer_capacity = data_length;
delete tss;
return ret == true ? RMW_RET_OK : RMW_RET_ERROR;
}
bool
get_datareader_qos(
DDSDomainParticipant * participant,
const rmw_qos_profile_t & qos_profile,
DDS_DataReaderQos & datareader_qos)
{
DDS_ReturnCode_t status = participant->get_default_datareader_qos(datareader_qos);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to get default datareader qos");
return false;
}
status = DDSPropertyQosPolicyHelper::add_property(
datareader_qos.property,
"dds.data_reader.history.memory_manager.fast_pool.pool_buffer_max_size",
"4096",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return false;
}
status = DDSPropertyQosPolicyHelper::add_property(
datareader_qos.property,
"reader_resource_limits.dynamically_allocate_fragmented_samples",
"1",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return false;
}
if (!set_entity_qos_from_profile(qos_profile, datareader_qos)) {
return false;
}
return true;
}
ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL
inline std::string
_create_type_name(
const void * untyped_members,
const std::string & sep)
{
auto members = static_cast<const MembersType *>(untyped_members);
if (!members) {
RMW_SET_ERROR_MSG("members handle is null");
return "";
}
return
std::string(members->package_name_) + "::" + sep + "::dds_::" + members->message_name_ + "_";
}
rmw_ret_t
rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result)
{
if (!gid1) {
RMW_SET_ERROR_MSG("gid1 is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
gid1,
gid1->implementation_identifier, opensplice_cpp_identifier,
return RMW_RET_ERROR)
if (!gid2) {
RMW_SET_ERROR_MSG("gid2 is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
gid2,
gid2->implementation_identifier, opensplice_cpp_identifier,
return RMW_RET_ERROR)
if (!result) {
RMW_SET_ERROR_MSG("result is null");
return RMW_RET_ERROR;
}
auto detail1 = reinterpret_cast<const OpenSplicePublisherGID *>(gid1->data);
if (!detail1) {
RMW_SET_ERROR_MSG("gid1 is invalid");
return RMW_RET_ERROR;
}
auto detail2 = reinterpret_cast<const OpenSplicePublisherGID *>(gid2->data);
if (!detail2) {
RMW_SET_ERROR_MSG("gid2 is invalid");
return RMW_RET_ERROR;
}
*result = detail1->publication_handle == detail2->publication_handle;
return RMW_RET_OK;
}
rmw_fastrtps_shared_cpp::TypeSupport *
_create_response_type_support(const void * untyped_members, const char * typesupport_identifier)
{
if (using_introspection_c_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *>(
untyped_members);
return new ResponseTypeSupport_c(members);
} else if (using_introspection_cpp_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *>(
untyped_members);
return new ResponseTypeSupport_cpp(members);
}
RMW_SET_ERROR_MSG("Unknown typesupport identifier");
return nullptr;
}
rmw_ret_t
get_service_names_and_types(
const char * implementation_identifier,
const rmw_node_t * node,
rcutils_allocator_t * allocator,
rmw_names_and_types_t * service_names_and_types)
{
if (!allocator) {
RMW_SET_ERROR_MSG("allocator is null");
return RMW_RET_INVALID_ARGUMENT;
}
if (!node) {
RMW_SET_ERROR_MSG("null node handle");
return RMW_RET_INVALID_ARGUMENT;
}
if (node->implementation_identifier != implementation_identifier) {
RMW_SET_ERROR_MSG("node handle is not from this rmw implementation");
return RMW_RET_ERROR;
}
rmw_ret_t ret = rmw_names_and_types_check_zero(service_names_and_types);
if (ret != RMW_RET_OK) {
return ret;
}
auto node_info = static_cast<ConnextNodeInfo *>(node->data);
if (!node_info) {
RMW_SET_ERROR_MSG("node info handle is null");
return RMW_RET_ERROR;
}
if (!node_info->publisher_listener) {
RMW_SET_ERROR_MSG("publisher listener handle is null");
return RMW_RET_ERROR;
}
if (!node_info->subscriber_listener) {
RMW_SET_ERROR_MSG("subscriber listener handle is null");
return RMW_RET_ERROR;
}
// combine publisher and subscriber information
std::map<std::string, std::set<std::string>> services;
node_info->publisher_listener->fill_service_names_and_types(services);
node_info->subscriber_listener->fill_service_names_and_types(services);
// Fill out service_names_and_types
if (!services.empty()) {
rmw_ret_t rmw_ret =
copy_services_to_names_and_types(services, allocator, service_names_and_types);
if (rmw_ret != RMW_RET_OK) {
return rmw_ret;
}
}
return RMW_RET_OK;
}
rmw_ret_t
destroy_guard_condition(const char * implementation_identifier,
rmw_guard_condition_t * guard_condition)
{
if (!guard_condition) {
RMW_SET_ERROR_MSG("guard condition handle is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
guard condition handle,
guard_condition->implementation_identifier, implementation_identifier,
return RMW_RET_ERROR)
auto result = RMW_RET_OK;
RMW_TRY_DESTRUCTOR(
static_cast<DDSGuardCondition *>(guard_condition->data)->~DDSGuardCondition(),
DDSGuardCondition, result = RMW_RET_ERROR)
rmw_free(guard_condition->data);
rmw_guard_condition_free(guard_condition);
return result;
}
rmw_node_t *
create_node(const char * implementation_identifier, const char * name, size_t domain_id)
{
DDSDomainParticipantFactory * dpf_ = DDSDomainParticipantFactory::get_instance();
if (!dpf_) {
RMW_SET_ERROR_MSG("failed to get participant factory");
return NULL;
}
// use loopback interface to enable cross vendor communication
DDS_DomainParticipantQos participant_qos;
DDS_ReturnCode_t status = dpf_->get_default_participant_qos(participant_qos);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to get default participant qos");
return NULL;
}
// forces local traffic to be sent over loopback,
// even if a more efficient transport (such as shared memory) is installed
// (in which case traffic will be sent over both transports)
status = DDSPropertyQosPolicyHelper::add_property(
participant_qos.property,
"dds.transport.UDPv4.builtin.ignore_loopback_interface",
"0",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return NULL;
}
status = DDSPropertyQosPolicyHelper::add_property(
participant_qos.property,
"dds.transport.use_510_compatible_locator_kinds",
"1",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return NULL;
}
DDS_DomainId_t domain = static_cast<DDS_DomainId_t>(domain_id);
DDSDomainParticipant * participant = dpf_->create_participant(
domain, participant_qos, NULL,
DDS_STATUS_MASK_NONE);
if (!participant) {
RMW_SET_ERROR_MSG("failed to create participant");
return NULL;
}
rmw_node_t * node_handle = nullptr;
ConnextNodeInfo * node_info = nullptr;
rmw_guard_condition_t * graph_guard_condition = nullptr;
CustomPublisherListener * publisher_listener = nullptr;
CustomSubscriberListener * subscriber_listener = nullptr;
void * buf = nullptr;
DDSDataReader * data_reader = nullptr;
DDSPublicationBuiltinTopicDataDataReader * builtin_publication_datareader = nullptr;
DDSSubscriptionBuiltinTopicDataDataReader * builtin_subscription_datareader = nullptr;
DDSSubscriber * builtin_subscriber = participant->get_builtin_subscriber();
if (!builtin_subscriber) {
RMW_SET_ERROR_MSG("builtin subscriber handle is null");
goto fail;
}
// setup publisher listener
data_reader = builtin_subscriber->lookup_datareader(DDS_PUBLICATION_TOPIC_NAME);
builtin_publication_datareader =
static_cast<DDSPublicationBuiltinTopicDataDataReader *>(data_reader);
if (!builtin_publication_datareader) {
RMW_SET_ERROR_MSG("builtin publication datareader handle is null");
goto fail;
}
graph_guard_condition = create_guard_condition(implementation_identifier);
if (!graph_guard_condition) {
RMW_SET_ERROR_MSG("failed to create graph guard condition");
goto fail;
}
buf = rmw_allocate(sizeof(CustomPublisherListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
publisher_listener, buf, goto fail, CustomPublisherListener,
implementation_identifier, graph_guard_condition)
buf = nullptr;
builtin_publication_datareader->set_listener(publisher_listener, DDS_DATA_AVAILABLE_STATUS);
data_reader = builtin_subscriber->lookup_datareader(DDS_SUBSCRIPTION_TOPIC_NAME);
builtin_subscription_datareader =
static_cast<DDSSubscriptionBuiltinTopicDataDataReader *>(data_reader);
if (!builtin_subscription_datareader) {
RMW_SET_ERROR_MSG("builtin subscription datareader handle is null");
goto fail;
}
// setup subscriber listener
buf = rmw_allocate(sizeof(CustomSubscriberListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
subscriber_listener, buf, goto fail, CustomSubscriberListener,
implementation_identifier, graph_guard_condition)
buf = nullptr;
builtin_subscription_datareader->set_listener(subscriber_listener, DDS_DATA_AVAILABLE_STATUS);
node_handle = rmw_node_allocate();
if (!node_handle) {
RMW_SET_ERROR_MSG("failed to allocate memory for node handle");
goto fail;
}
node_handle->implementation_identifier = implementation_identifier;
node_handle->data = participant;
node_handle->name =
reinterpret_cast<const char *>(rmw_allocate(sizeof(char) * strlen(name) + 1));
if (!node_handle->name) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
memcpy(const_cast<char *>(node_handle->name), name, strlen(name) + 1);
buf = rmw_allocate(sizeof(ConnextNodeInfo));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(node_info, buf, goto fail, ConnextNodeInfo)
buf = nullptr;
node_info->participant = participant;
node_info->publisher_listener = publisher_listener;
node_info->subscriber_listener = subscriber_listener;
node_info->graph_guard_condition = graph_guard_condition;
node_handle->implementation_identifier = implementation_identifier;
node_handle->data = node_info;
return node_handle;
fail:
status = dpf_->delete_participant(participant);
if (status != DDS_RETCODE_OK) {
std::stringstream ss;
ss << "leaking participant while handling failure at " <<
__FILE__ << ":" << __LINE__;
(std::cerr << ss.str()).flush();
}
if (graph_guard_condition) {
rmw_ret_t ret = destroy_guard_condition(implementation_identifier, graph_guard_condition);
if (ret != RMW_RET_OK) {
std::stringstream ss;
ss << "failed to destroy guard condition while handling failure at " <<
__FILE__ << ":" << __LINE__;
(std::cerr << ss.str()).flush();
}
}
if (publisher_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
publisher_listener->~CustomPublisherListener(), CustomPublisherListener)
rmw_free(publisher_listener);
}
if (subscriber_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener)
rmw_free(subscriber_listener);
}
if (node_handle) {
if (node_handle->name) {
rmw_free(const_cast<char *>(node_handle->name));
}
rmw_free(node_handle);
}
if (node_info) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->~ConnextNodeInfo(), ConnextNodeInfo)
rmw_free(node_info);
}
if (buf) {
rmw_free(buf);
}
return NULL;
}
rmw_ret_t
__rmw_get_node_names(
const char * identifier,
const rmw_node_t * node,
rcutils_string_array_t * node_names,
rcutils_string_array_t * node_namespaces)
{
if (!node) {
RMW_SET_ERROR_MSG("null node handle");
return RMW_RET_ERROR;
}
if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) {
return RMW_RET_ERROR;
}
if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) {
return RMW_RET_ERROR;
}
// Get participant pointer from node
if (node->implementation_identifier != identifier) {
RMW_SET_ERROR_MSG("node handle not from this implementation");
return RMW_RET_ERROR;
}
auto impl = static_cast<CustomParticipantInfo *>(node->data);
auto participant_names = impl->listener->get_discovered_names();
auto participant_ns = impl->listener->get_discovered_namespaces();
rcutils_allocator_t allocator = rcutils_get_default_allocator();
rcutils_ret_t rcutils_ret =
rcutils_string_array_init(node_names, participant_names.size() + 1, &allocator);
if (rcutils_ret != RCUTILS_RET_OK) {
RMW_SET_ERROR_MSG(rcutils_get_error_string().str);
goto fail;
}
rcutils_ret =
rcutils_string_array_init(node_namespaces, participant_names.size() + 1, &allocator);
if (rcutils_ret != RCUTILS_RET_OK) {
RMW_SET_ERROR_MSG(rcutils_get_error_string().str);
goto fail;
}
for (size_t i = 0; i < participant_names.size() + 1; ++i) {
if (0 == i) {
node_names->data[i] = rcutils_strdup(node->name, allocator);
node_namespaces->data[i] = rcutils_strdup(node->namespace_, allocator);
} else {
node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator);
node_namespaces->data[i] = rcutils_strdup(participant_ns[i - 1].c_str(), allocator);
}
if (!node_names->data[i] || !node_namespaces->data[i]) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
}
return RMW_RET_OK;
fail:
if (node_names) {
rcutils_ret = rcutils_string_array_fini(node_names);
if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
if (node_namespaces) {
rcutils_ret = rcutils_string_array_fini(node_namespaces);
if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
return RMW_RET_BAD_ALLOC;
}
DDS_TypeCode * create_type_code(
std::string type_name, const void * untyped_members, const char * introspection_identifier)
{
auto members = static_cast<const MembersType *>(untyped_members);
if (!members) {
RMW_SET_ERROR_MSG("members handle is null");
return NULL;
}
DDS_TypeCodeFactory * factory = DDS_TypeCodeFactory::get_instance();
if (!factory) {
RMW_SET_ERROR_MSG("failed to get typecode factory");
return NULL;
}
// Past this point, a failure results in unrolling code in the goto fail block.
DDS_StructMemberSeq struct_members;
DDS_ExceptionCode_t ex = DDS_NO_EXCEPTION_CODE;
// Start initializing elements.
DDS_TypeCode * type_code = factory->create_struct_tc(type_name.c_str(), struct_members, ex);
if (!type_code || ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to create struct typecode");
goto fail;
}
USING_INTROSPECTION_TYPEIDS()
for (uint32_t i = 0; i < members->member_count_; ++i) {
auto * member = members->members_ + i;
const DDS_TypeCode * member_type_code = nullptr;
DDS_TypeCode * member_type_code_non_const = nullptr;
switch (member->type_id_) {
case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL:
member_type_code = factory->get_primitive_tc(DDS_TK_BOOLEAN);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE:
member_type_code = factory->get_primitive_tc(DDS_TK_OCTET);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR:
member_type_code = factory->get_primitive_tc(DDS_TK_CHAR);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32:
member_type_code = factory->get_primitive_tc(DDS_TK_FLOAT);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64:
member_type_code = factory->get_primitive_tc(DDS_TK_DOUBLE);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8:
member_type_code = factory->get_primitive_tc(DDS_TK_OCTET);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8:
member_type_code = factory->get_primitive_tc(DDS_TK_OCTET);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16:
member_type_code = factory->get_primitive_tc(DDS_TK_SHORT);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16:
member_type_code = factory->get_primitive_tc(DDS_TK_USHORT);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32:
member_type_code = factory->get_primitive_tc(DDS_TK_LONG);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32:
member_type_code = factory->get_primitive_tc(DDS_TK_ULONG);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64:
member_type_code = factory->get_primitive_tc(DDS_TK_LONGLONG);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64:
member_type_code = factory->get_primitive_tc(DDS_TK_ULONGLONG);
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
{
DDS_UnsignedLong max_string_size;
if (member->string_upper_bound_) {
if (member->string_upper_bound_ > (std::numeric_limits<DDS_UnsignedLong>::max)()) {
RMW_SET_ERROR_MSG(
"failed to create string typecode since the upper bound exceeds the DDS type");
goto fail;
}
max_string_size = static_cast<DDS_UnsignedLong>(member->string_upper_bound_);
} else {
max_string_size = RTI_INT32_MAX;
}
member_type_code_non_const = factory->create_string_tc(max_string_size, ex);
member_type_code = member_type_code_non_const;
}
if (!member_type_code || ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to create string typecode");
goto fail;
}
break;
case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
{
if (!member->members_) {
RMW_SET_ERROR_MSG("members handle is null");
return NULL;
}
auto sub_members = static_cast<const MembersType *>(member->members_->data);
if (!sub_members) {
RMW_SET_ERROR_MSG("sub members handle is null");
return NULL;
}
std::string field_type_name = _create_type_name<MembersType>(sub_members, "msg");
member_type_code = create_type_code<MembersType>(field_type_name, sub_members,
introspection_identifier);
if (!member_type_code) {
// error string was set within the function
goto fail;
}
}
break;
default:
RMW_SET_ERROR_MSG(
(std::string("unknown type id ") + std::to_string(member->type_id_)).c_str());
goto fail;
}
if (!member_type_code) {
RMW_SET_ERROR_MSG("failed to create typecode");
goto fail;
}
if (member->is_array_) {
if (member->array_size_) {
if (member->array_size_ > (std::numeric_limits<DDS_UnsignedLong>::max)()) {
RMW_SET_ERROR_MSG("failed to create array typecode since the size exceeds the DDS type");
goto fail;
}
DDS_UnsignedLong array_size = static_cast<DDS_UnsignedLong>(member->array_size_);
if (!member->is_upper_bound_) {
member_type_code_non_const = factory->create_array_tc(array_size, member_type_code, ex);
member_type_code = member_type_code_non_const;
if (!member_type_code || ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to create array typecode");
goto fail;
}
} else {
member_type_code_non_const =
factory->create_sequence_tc(array_size, member_type_code, ex);
member_type_code = member_type_code_non_const;
if (!member_type_code || ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to create sequence typecode");
goto fail;
}
}
} else {
member_type_code_non_const = factory->create_sequence_tc(
RTI_INT32_MAX, member_type_code, ex);
member_type_code = member_type_code_non_const;
if (!member_type_code || ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to create sequence typecode");
goto fail;
}
}
}
auto zero_based_index = type_code->add_member(
(std::string(member->name_) + "_").c_str(),
i,
member_type_code,
DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
if (ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to add member");
goto fail;
}
if (zero_based_index != i) {
RMW_SET_ERROR_MSG("unexpected member index");
return NULL;
}
}
// since empty message definitions are not supported
// we have to add the same dummy field as in rosidl_generator_dds_idl
if (members->member_count_ == 0) {
const DDS_TypeCode * member_type_code;
member_type_code = factory->get_primitive_tc(DDS_TK_BOOLEAN);
if (!member_type_code) {
RMW_SET_ERROR_MSG("failed to get primitive typecode");
goto fail;
}
type_code->add_member("_dummy", DDS_TYPECODE_MEMBER_ID_INVALID,
member_type_code,
DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
if (ex != DDS_NO_EXCEPTION_CODE) {
RMW_SET_ERROR_MSG("failed to add member");
goto fail;
}
}
DDS_StructMemberSeq_finalize(&struct_members);
return type_code;
fail:
if (type_code) {
if (factory) {
DDS_ExceptionCode_t exc = DDS_NO_EXCEPTION_CODE;
factory->delete_tc(type_code, exc);
if (exc != DDS_NO_EXCEPTION_CODE) {
std::stringstream ss;
ss << "failed to delete type code during handling of failure at " <<
__FILE__ << ":" << __LINE__ << '\n';
(std::cerr << ss.str()).flush();
}
} else {
std::stringstream ss;
ss << "leaking type code during handling of failure at " <<
__FILE__ << ":" << __LINE__ << '\n';
(std::cerr << ss.str()).flush();
}
}
return nullptr;
}
static bool
take(
DDS::DataReader * dds_data_reader,
bool ignore_local_publications,
rcutils_uint8_array_t * cdr_stream,
bool * taken,
void * sending_publication_handle,
rmw_subscription_allocation_t * allocation)
{
(void) allocation;
if (!dds_data_reader) {
RMW_SET_ERROR_MSG("dds_data_reader is null");
return false;
}
if (!cdr_stream) {
RMW_SET_ERROR_MSG("cdr stream handle is null");
return false;
}
if (!taken) {
RMW_SET_ERROR_MSG("taken handle is null");
return false;
}
ConnextStaticSerializedDataDataReader * data_reader =
ConnextStaticSerializedDataDataReader::narrow(dds_data_reader);
if (!data_reader) {
RMW_SET_ERROR_MSG("failed to narrow data reader");
return false;
}
ConnextStaticSerializedDataSeq dds_messages;
DDS::SampleInfoSeq sample_infos;
bool ignore_sample = false;
DDS::ReturnCode_t status = data_reader->take(
dds_messages,
sample_infos,
1,
DDS::ANY_SAMPLE_STATE,
DDS::ANY_VIEW_STATE,
DDS::ANY_INSTANCE_STATE);
if (status == DDS::RETCODE_NO_DATA) {
data_reader->return_loan(dds_messages, sample_infos);
*taken = false;
return true;
}
if (status != DDS::RETCODE_OK) {
RMW_SET_ERROR_MSG("take failed");
data_reader->return_loan(dds_messages, sample_infos);
return false;
}
DDS::SampleInfo & sample_info = sample_infos[0];
if (!sample_info.valid_data) {
// skip sample without data
ignore_sample = true;
} else if (ignore_local_publications) {
// compare the lower 12 octets of the guids from the sender and this receiver
// if they are equal the sample has been sent from this process and should be ignored
DDS::GUID_t sender_guid = sample_info.original_publication_virtual_guid;
DDS::InstanceHandle_t receiver_instance_handle = dds_data_reader->get_instance_handle();
ignore_sample = true;
for (size_t i = 0; i < 12; ++i) {
DDS::Octet * sender_element = &(sender_guid.value[i]);
DDS::Octet * receiver_element =
&(reinterpret_cast<DDS::Octet *>(&receiver_instance_handle)[i]);
if (*sender_element != *receiver_element) {
ignore_sample = false;
break;
}
}
}
if (sample_info.valid_data && sending_publication_handle) {
*static_cast<DDS::InstanceHandle_t *>(sending_publication_handle) =
sample_info.publication_handle;
}
if (!ignore_sample) {
cdr_stream->buffer_length = dds_messages[0].serialized_data.length();
// TODO(karsten1987): This malloc has to go!
cdr_stream->buffer =
reinterpret_cast<uint8_t *>(malloc(cdr_stream->buffer_length * sizeof(uint8_t)));
if (cdr_stream->buffer_length > (std::numeric_limits<unsigned int>::max)()) {
RMW_SET_ERROR_MSG("cdr_stream->buffer_length unexpectedly larger than max unsiged int value");
data_reader->return_loan(dds_messages, sample_infos);
*taken = false;
return false;
}
for (unsigned int i = 0; i < static_cast<unsigned int>(cdr_stream->buffer_length); ++i) {
cdr_stream->buffer[i] = dds_messages[0].serialized_data[i];
}
*taken = true;
} else {
*taken = false;
}
data_reader->return_loan(dds_messages, sample_infos);
return status == DDS::RETCODE_OK;
}
rmw_ret_t
destroy_node(const char * implementation_identifier, rmw_node_t * node)
{
if (!node) {
RMW_SET_ERROR_MSG("node handle is null");
return RMW_RET_ERROR;
}
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
node handle,
node->implementation_identifier, implementation_identifier,
return RMW_RET_ERROR)
DDSDomainParticipantFactory * dpf_ = DDSDomainParticipantFactory::get_instance();
if (!dpf_) {
RMW_SET_ERROR_MSG("failed to get participant factory");
return RMW_RET_ERROR;
}
auto node_info = static_cast<ConnextNodeInfo *>(node->data);
if (!node_info) {
RMW_SET_ERROR_MSG("node info handle is null");
return RMW_RET_ERROR;
}
auto participant = static_cast<DDSDomainParticipant *>(node_info->participant);
if (!participant) {
RMW_SET_ERROR_MSG("participant handle is null");
}
// This unregisters types and destroys topics which were shared between
// publishers and subscribers and could not be cleaned up in the delete functions.
if (participant->delete_contained_entities() != DDS::RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to delete contained entities of participant");
return RMW_RET_ERROR;
}
DDS_ReturnCode_t ret = dpf_->delete_participant(participant);
if (ret != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to delete participant");
return RMW_RET_ERROR;
}
if (node_info->publisher_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->publisher_listener->~CustomPublisherListener(), CustomPublisherListener)
rmw_free(node_info->publisher_listener);
node_info->publisher_listener = nullptr;
}
if (node_info->subscriber_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener)
rmw_free(node_info->subscriber_listener);
node_info->subscriber_listener = nullptr;
}
if (node_info->graph_guard_condition) {
rmw_ret_t ret =
destroy_guard_condition(implementation_identifier, node_info->graph_guard_condition);
if (ret != RMW_RET_OK) {
RMW_SET_ERROR_MSG("failed to delete graph guard condition");
return RMW_RET_ERROR;
}
node_info->graph_guard_condition = nullptr;
}
rmw_free(node_info);
node->data = nullptr;
rmw_free(const_cast<char *>(node->name));
node->name = nullptr;
rmw_node_free(node);
return RMW_RET_OK;
}
bool
get_datawriter_qos(
const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr)
{
switch (qos_policies.history) {
case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS;
break;
case RMW_QOS_POLICY_HISTORY_KEEP_ALL:
pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS;
break;
case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
break;
default:
RMW_SET_ERROR_MSG("Unknown QoS history policy");
return false;
}
switch (qos_policies.durability) {
case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:
pattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS;
break;
case RMW_QOS_POLICY_DURABILITY_VOLATILE:
pattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS;
break;
case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT:
break;
default:
RMW_SET_ERROR_MSG("Unknown QoS durability policy");
return false;
}
switch (qos_policies.reliability) {
case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT:
pattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS;
break;
case RMW_QOS_POLICY_RELIABILITY_RELIABLE:
pattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS;
break;
case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT:
break;
default:
RMW_SET_ERROR_MSG("Unknown QoS reliability policy");
return false;
}
if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) {
pattr.topic.historyQos.depth = static_cast<int32_t>(qos_policies.depth);
}
// ensure the history depth is at least the requested queue size
assert(pattr.topic.historyQos.depth >= 0);
if (
eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == pattr.topic.historyQos.kind &&
static_cast<size_t>(pattr.topic.historyQos.depth) < qos_policies.depth)
{
if (qos_policies.depth > (std::numeric_limits<int32_t>::max)()) {
RMW_SET_ERROR_MSG(
"failed to set history depth since the requested queue size exceeds the DDS type");
return false;
}
pattr.topic.historyQos.depth = static_cast<int32_t>(qos_policies.depth);
}
return true;
}
