diff --git a/README.md b/README.md index 4913ee874..7f881d528 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,7 @@ You can however set it to `rmw_fastrtps_dynamic_cpp` using the environment varia * Publication mode: `ASYNCHRONOUS_PUBLISH_MODE` However, it is possible to fully configure Fast DDS (including the history memory policy and the publication mode) using an XML file as described in [Fast DDS documentation](https://fast-dds.docs.eprosima.com/en/latest/fastdds/xml_configuration/xml_configuration.html). -If you want to modify the history memory policy or the publication mode you must set environment variable `RMW_FASTRTPS_USE_QOS_FROM_XML` to 1 (it is set to 0 by default). +Bear in mind that if you want to modify the history memory policy or the publication mode you must set environment variable `RMW_FASTRTPS_USE_QOS_FROM_XML` to 1 (it is set to 0 by default) besides defining the XML file. This tells `rmw_fastrtps` that it should override both the history memory policy and the publication mode using the XML. Bear in mind that if you set this environment variable but do not give a value to either of these policies, defaults will be used. Current Fast-DDS defaults are: @@ -44,6 +44,23 @@ You have two ways of telling you ROS 2 application which XML to use: 1. Placing your XML file in the running directory under the name `DEFAULT_FASTRTPS_PROFILES.xml`. 2. Setting environment variable `FASTRTPS_DEFAULT_PROFILES_FILE` to your XML file. +### Change publication mode + +Another way to change easily the publication mode without the need of defining a XML file is to use the environment variable `RMW_FASTRTPS_PUBLICATION_MODE`. +This variable has lower precedence than `RMW_FASTRTPS_USE_QOS_FROM_XML`. +Therefore, it is only taken into account when `RMW_FASTRTPS_USE_QOS_FROM_XML` is not set (or given a value different than `1`). +The admissible values are: +* `ASYNCHRONOUS`: asynchronous publication mode. +Setting this mode implies that when the publisher invokes the write operation, the data is copied into a queue, a notification about the addition to the queue is performed, and control of the thread is returned to the user before the data is actually sent. +A background thread (asynchronous thread) is in turn in charge of consuming the queue and sending the data to every matched reader. +* `SYNCHRONOUS`: synchronous publication mode. +Setting this mode implies that the data is sent directly within the context of the user thread. +This entails that any blocking call occurring during the write operation would block the user thread, thus preventing the application with continuing its operation. +It is important to note that this mode typically yields higher throughput rates at lower latencies, since the notification and context switching between threads is not present. +* `AUTO`: let Fast DDS select the publication mode. This implies using the publication mode set in the XML file or, failing that, the default value set in Fast DDS (which currently is set to `SYNCHRONOUS`). + +If `RMW_FASTRTPS_PUBLICATION_MODE` is not set, then `rmw_fastrtps_cpp` and `rmw_fastrtps_dynamic_cpp` behave as if it were set to `ASYNCHRONOUS`. + ## Example The following example configures Fast DDS to publish synchronously, and to have a pre-allocated history that can be expanded whenever it gets filled. diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index ccece0ffd..06032e240 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -135,9 +135,13 @@ rmw_fastrtps_cpp::create_publisher( } if (!participant_info->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } } publisherParam.topic.topicKind = diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 7ca96164e..642c829da 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -165,9 +165,13 @@ rmw_create_client( qos_policies, ros_service_response_prefix, service_name, "Reply"); if (!participant_info->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } } publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 144b24f04..220fe26a0 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -174,9 +174,13 @@ rmw_create_service( qos_policies, ros_service_requester_prefix, service_name, "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; + if (impl->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (impl->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } } publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 68328611e..3e067b1a2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -145,9 +145,13 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } if (!participant_info->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } } publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 282ee5d94..e7f24a769 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -183,9 +183,13 @@ rmw_create_client( qos_policies, ros_service_response_prefix, service_name, "Reply"); if (!participant_info->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } } publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 14923a29c..8234e8ab7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -192,9 +192,13 @@ rmw_create_service( qos_policies, ros_service_requester_prefix, service_name, "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; + if (impl->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (impl->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } } publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; 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 c20969d79..8b2106b37 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 @@ -42,6 +42,13 @@ using rmw_dds_common::operator<<; class ParticipantListener; +enum class publishing_mode_t +{ + ASYNCHRONOUS, // Asynchronous publishing mode + SYNCHRONOUS, // Synchronous publishing mode + AUTO // Use publishing mode set in XML file or Fast DDS default +}; + typedef struct CustomParticipantInfo { eprosima::fastrtps::Participant * participant; @@ -53,6 +60,7 @@ typedef struct CustomParticipantInfo // their settings are going to be overwritten by code // with the default configuration. bool leave_middleware_default_qos; + publishing_mode_t publishing_mode; } CustomParticipantInfo; class ParticipantListener : public eprosima::fastrtps::ParticipantListener diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 6f4a1d56b..2e4ccb516 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -94,6 +94,7 @@ __create_participant( const char * identifier, ParticipantAttributes participantAttrs, bool leave_middleware_default_qos, + publishing_mode_t publishing_mode, rmw_dds_common::Context * common_context) { // Declare everything before beginning to create things. @@ -122,6 +123,7 @@ __create_participant( goto fail; } participant_info->leave_middleware_default_qos = leave_middleware_default_qos; + participant_info->publishing_mode = publishing_mode; participant_info->participant = participant; participant_info->listener = listener; @@ -185,6 +187,7 @@ rmw_fastrtps_shared_cpp::create_participant( participantAttrs.rtps.setName(enclave); bool leave_middleware_default_qos = false; + publishing_mode_t publishing_mode = publishing_mode_t::ASYNCHRONOUS; const char * env_value; const char * error_str; error_str = rcutils_get_env("RMW_FASTRTPS_USE_QOS_FROM_XML", &env_value); @@ -195,6 +198,26 @@ rmw_fastrtps_shared_cpp::create_participant( if (env_value != nullptr) { leave_middleware_default_qos = strcmp(env_value, "1") == 0; } + if (!leave_middleware_default_qos) { + error_str = rcutils_get_env("RMW_FASTRTPS_PUBLICATION_MODE", &env_value); + if (error_str != NULL) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Error getting env var: %s\n", error_str); + return nullptr; + } + if (env_value != nullptr) { + // Synchronous publishing + if (strcmp(env_value, "SYNCHRONOUS") == 0) { + publishing_mode = publishing_mode_t::SYNCHRONOUS; + } else if (strcmp(env_value, "AUTO") == 0) { + publishing_mode = publishing_mode_t::AUTO; + } else if (strcmp(env_value, "ASYNCHRONOUS") != 0 && strcmp(env_value, "") != 0) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "Value %s unknown for environment variable RMW_FASTRTPS_PUBLICATION_MODE" + ". Using default ASYNCHRONOUS publishing mode.", env_value); + } + } + } // allow reallocation to support discovery messages bigger than 5000 bytes if (!leave_middleware_default_qos) { participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = @@ -257,6 +280,7 @@ rmw_fastrtps_shared_cpp::create_participant( identifier, participantAttrs, leave_middleware_default_qos, + publishing_mode, common_context); }