diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 171a72cfb..409a91f1c 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -120,10 +120,13 @@ rmw_create_client( _register_type(participant, info->response_type_support_); } + if (!impl->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; subscriberParam.topic.topicDataType = response_type_name; - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; if (!qos_policies->avoid_ros_namespace_conventions) { subscriberParam.topic.topicName = std::string(ros_service_response_prefix) + service_name; } else { @@ -131,11 +134,14 @@ rmw_create_client( } subscriberParam.topic.topicName += "Reply"; + if (!impl->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; publisherParam.topic.topicDataType = request_type_name; - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; if (!qos_policies->avoid_ros_namespace_conventions) { publisherParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name; } else { diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 5907590fd..520afdf6a 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -113,9 +113,12 @@ rmw_create_publisher( _register_type(participant, info->type_support_); } - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!impl->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; publisherParam.topic.topicDataType = type_name; if (!qos_policies->avoid_ros_namespace_conventions) { diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index af81304e4..80a8739ff 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -132,9 +132,12 @@ rmw_create_service( _register_type(participant, info->response_type_support_); } + if (!impl->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; subscriberParam.topic.topicDataType = request_type_name; if (!qos_policies->avoid_ros_namespace_conventions) { subscriberParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name; @@ -143,11 +146,14 @@ rmw_create_service( } subscriberParam.topic.topicName += "Request"; + if (!impl->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; publisherParam.topic.topicDataType = response_type_name; - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; if (!qos_policies->avoid_ros_namespace_conventions) { publisherParam.topic.topicName = std::string(ros_service_response_prefix) + service_name; } else { diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 998b859f8..27ad9e374 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -116,8 +116,11 @@ rmw_create_subscription( _register_type(participant, info->type_support_); } - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!impl->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; subscriberParam.topic.topicDataType = type_name; if (!qos_policies->avoid_ros_namespace_conventions) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index da3c6cda0..7adbf62f0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -125,10 +125,13 @@ rmw_create_client( _register_type(participant, info->response_type_support_); } + if (!impl->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; subscriberParam.topic.topicDataType = response_type_name; - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; if (!qos_policies->avoid_ros_namespace_conventions) { subscriberParam.topic.topicName = std::string(ros_service_response_prefix) + service_name; } else { @@ -136,11 +139,14 @@ rmw_create_client( } subscriberParam.topic.topicName += "Reply"; + if (!impl->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; publisherParam.topic.topicDataType = request_type_name; - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; if (!qos_policies->avoid_ros_namespace_conventions) { publisherParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name; } else { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index ae44cfe03..f279dcbfc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -108,9 +108,12 @@ rmw_create_publisher( _register_type(participant, info->type_support_); } - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!impl->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; publisherParam.topic.topicDataType = type_name; if (!qos_policies->avoid_ros_namespace_conventions) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 2de1d7645..42495c7d0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -137,9 +137,12 @@ rmw_create_service( _register_type(participant, info->response_type_support_); } + if (!impl->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; subscriberParam.topic.topicDataType = request_type_name; if (!qos_policies->avoid_ros_namespace_conventions) { subscriberParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name; @@ -148,11 +151,14 @@ rmw_create_service( } subscriberParam.topic.topicName += "Request"; + if (!impl->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; publisherParam.topic.topicDataType = response_type_name; - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; if (!qos_policies->avoid_ros_namespace_conventions) { publisherParam.topic.topicName = std::string(ros_service_response_prefix) + service_name; } else { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index a421424fe..a5c9e7e52 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -112,8 +112,11 @@ rmw_create_subscription( _register_type(participant, info->type_support_); } - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!impl->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; subscriberParam.topic.topicDataType = type_name; if (!qos_policies->avoid_ros_namespace_conventions) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 4e086435d..163eb9484 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -37,6 +37,13 @@ typedef struct CustomParticipantInfo ReaderInfo * secondarySubListener; WriterInfo * secondaryPubListener; rmw_guard_condition_t * graph_guard_condition; + + // Flag to establish if the QoS of the participant, + // its publishers and its subscribers are going + // to be configured only from an XML file or if + // their settings are going to be overwritten by code + // with the default configuration. + bool leave_middleware_default_qos; } CustomParticipantInfo; class ParticipantListener : public eprosima::fastrtps::ParticipantListener diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp index c106f5db6..e9dc8b207 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp @@ -104,6 +104,25 @@ create_node( try { node_impl = new CustomParticipantInfo(); + + node_impl->leave_middleware_default_qos = false; + const char * env_var = "RMW_FASTRTPS_USE_QOS_FROM_XML"; + // Check if the configuration from XML has been enabled from + // the RMW_FASTRTPS_USE_QOS_FROM_XML env variable. + char * config_env_val = nullptr; +#ifndef _WIN32 + config_env_val = getenv(env_var); + if (config_env_val != nullptr) { + node_impl->leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; + } +#else + size_t config_env_val_size; + _dupenv_s(&config_env_val, &config_env_val_size, env_var); + if (config_env_val != nullptr) { + node_impl->leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; + } + free(config_env_val); +#endif } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("failed to allocate node impl struct"); goto fail; @@ -246,11 +265,32 @@ __rmw_create_node( // since the participant name is not part of the DDS spec participantAttrs.rtps.setName(name); + bool leave_middleware_default_qos = false; + const char * env_var = "RMW_FASTRTPS_USE_QOS_FROM_XML"; + // Check if the configuration from XML has been enabled from + // the RMW_FASTRTPS_USE_QOS_FROM_XML env variable. + char * config_env_val = nullptr; +#ifndef _WIN32 + config_env_val = getenv(env_var); + if (config_env_val != nullptr) { + leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; + } +#else + size_t config_env_val_size; + _dupenv_s(&config_env_val, &config_env_val_size, env_var); + if (config_env_val != nullptr) { + leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; + } + free(config_env_val); +#endif + // allow reallocation to support discovery messages bigger than 5000 bytes - participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - participantAttrs.rtps.builtin.writerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!leave_middleware_default_qos) { + participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + participantAttrs.rtps.builtin.writerHistoryMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } size_t length = strlen(name) + strlen("name=;") + strlen(namespace_) + strlen("namespace=;") + 1;