From efe8dbc2d377ca95f8df1e42215cb059e9c70162 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 15 Apr 2022 16:42:26 -0700 Subject: [PATCH 1/2] Handle 'best_available' QoS policies If users set certain QoS policies to BEST_AVAILABLE, then we query the graph for endpoint info. The policy is updated such that it is compatible with all endpoints while maintaining the highest possible level of service. Signed-off-by: Jacob Perron --- rmw_fastrtps_cpp/src/rmw_publisher.cpp | 15 ++++++++++++++- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 14 +++++++++++++- rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp | 14 +++++++++++++- rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp | 14 +++++++++++++- 4 files changed, 53 insertions(+), 4 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index f607f1fe4e..9962a93029 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -17,10 +17,13 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/get_topic_endpoint_info.h" #include "rmw/rmw.h" #include "rmw/impl/cpp/macros.hpp" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/publisher.hpp" @@ -72,6 +75,16 @@ rmw_create_publisher( node->implementation_identifier, eprosima_fastrtps_identifier, return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + + // Adapt any 'best available' QoS options + rmw_qos_profile_t adapted_qos_policies = *qos_policies; + rmw_ret_t ret = rmw_dds_common::qos_profile_get_best_available_for_topic_publisher( + node, topic_name, &adapted_qos_policies, rmw_get_subscriptions_info_by_topic); + if (RMW_RET_OK != ret) { + return nullptr; + } + auto participant_info = static_cast(node->context->impl->participant_info); @@ -80,7 +93,7 @@ rmw_create_publisher( participant_info, type_supports, topic_name, - qos_policies, + &adapted_qos_policies, publisher_options, false, true); diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index c254660846..2793f55aa4 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -18,8 +18,11 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/get_topic_endpoint_info.h" #include "rmw/rmw.h" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -68,6 +71,15 @@ rmw_create_subscription( node->implementation_identifier, eprosima_fastrtps_identifier, return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + + // Adapt any 'best available' QoS options + rmw_qos_profile_t adapted_qos_policies = *qos_policies; + rmw_ret_t ret = rmw_dds_common::qos_profile_get_best_available_for_topic_subscription( + node, topic_name, &adapted_qos_policies, rmw_get_publishers_info_by_topic); + if (RMW_RET_OK != ret) { + return nullptr; + } auto participant_info = static_cast(node->context->impl->participant_info); @@ -75,7 +87,7 @@ rmw_create_subscription( participant_info, type_supports, topic_name, - qos_policies, + &adapted_qos_policies, subscription_options, false, // use no keyed topic true); // create subscription listener diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 9eb2c6c5c8..5be8b26ca8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -16,10 +16,13 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/get_topic_endpoint_info.h" #include "rmw/rmw.h" #include "rmw/impl/cpp/macros.hpp" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/publisher.hpp" @@ -74,6 +77,15 @@ rmw_create_publisher( node->implementation_identifier, eprosima_fastrtps_identifier, return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + + // Adapt any 'best available' QoS options + rmw_qos_profile_t adapted_qos_policies = *qos_policies; + rmw_ret_t ret = rmw_dds_common::qos_profile_get_best_available_for_topic_publisher( + node, topic_name, &adapted_qos_policies, rmw_get_subscriptions_info_by_topic); + if (RMW_RET_OK != ret) { + return nullptr; + } auto participant_info = static_cast(node->context->impl->participant_info); @@ -81,7 +93,7 @@ rmw_create_publisher( participant_info, type_supports, topic_name, - qos_policies, + &adapted_qos_policies, publisher_options, false, true); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index d3059c42a2..1fe1780449 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -17,8 +17,11 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" +#include "rmw/get_topic_endpoint_info.h" #include "rmw/rmw.h" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -70,6 +73,15 @@ rmw_create_subscription( node->implementation_identifier, eprosima_fastrtps_identifier, return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + + // Adapt any 'best available' QoS options + rmw_qos_profile_t adapted_qos_policies = *qos_policies; + rmw_ret_t ret = rmw_dds_common::qos_profile_get_best_available_for_topic_subscription( + node, topic_name, &adapted_qos_policies, rmw_get_publishers_info_by_topic); + if (RMW_RET_OK != ret) { + return nullptr; + } auto participant_info = static_cast(node->context->impl->participant_info); @@ -78,7 +90,7 @@ rmw_create_subscription( participant_info, type_supports, topic_name, - qos_policies, + &adapted_qos_policies, subscription_options, false, true); From 9bb3bdf960d34ddff5061d55338554990e0bca65 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 25 Apr 2022 17:13:37 -0700 Subject: [PATCH 2/2] Handle best available policies for services and clients Signed-off-by: Jacob Perron --- rmw_fastrtps_cpp/src/rmw_client.cpp | 17 +++++++++++------ rmw_fastrtps_cpp/src/rmw_service.cpp | 17 +++++++++++------ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 17 +++++++++++------ rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 17 +++++++++++------ 4 files changed, 44 insertions(+), 24 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index c71824383f..72c7ea4dd1 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -38,6 +38,8 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/names.hpp" @@ -88,9 +90,12 @@ rmw_create_client( } } + rmw_qos_profile_t adapted_qos_policies = + rmw_dds_common::qos_profile_update_best_available_for_services(*qos_policies); + ///// // Check RMW QoS - if (!is_valid_qos(*qos_policies)) { + if (!is_valid_qos(adapted_qos_policies)) { RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); return nullptr; } @@ -143,9 +148,9 @@ rmw_create_client( std::string response_type_name = _create_type_name(response_members); std::string request_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + &adapted_qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); std::string response_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + &adapted_qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; @@ -258,7 +263,7 @@ rmw_create_client( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) { + if (!get_topic_qos(adapted_qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); return nullptr; } @@ -315,7 +320,7 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -365,7 +370,7 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(*qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index cac7dd83f3..e99fcccf9d 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -44,6 +44,8 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/names.hpp" @@ -93,9 +95,12 @@ rmw_create_service( } } + rmw_qos_profile_t adapted_qos_policies = + rmw_dds_common::qos_profile_update_best_available_for_services(*qos_policies); + ///// // Check RMW QoS - if (!is_valid_qos(*qos_policies)) { + if (!is_valid_qos(adapted_qos_policies)) { RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); return nullptr; } @@ -148,9 +153,9 @@ rmw_create_service( std::string response_type_name = _create_type_name(response_members); std::string request_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + &adapted_qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); std::string response_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + &adapted_qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; @@ -260,7 +265,7 @@ rmw_create_service( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) { + if (!get_topic_qos(adapted_qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); return nullptr; } @@ -314,7 +319,7 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -368,7 +373,7 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(*qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index b72852ac00..765e0df297 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -36,6 +36,8 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rmw_dds_common/qos.hpp" + #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" @@ -91,9 +93,12 @@ rmw_create_client( } } + rmw_qos_profile_t adapted_qos_policies = + rmw_dds_common::qos_profile_update_best_available_for_services(*qos_policies); + ///// // Check RMW QoS - if (!is_valid_qos(*qos_policies)) { + if (!is_valid_qos(adapted_qos_policies)) { RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); return nullptr; } @@ -150,9 +155,9 @@ rmw_create_client( untyped_response_members, type_support->typesupport_identifier); std::string response_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + &adapted_qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); std::string request_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + &adapted_qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; @@ -289,7 +294,7 @@ rmw_create_client( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) { + if (!get_topic_qos(adapted_qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); return nullptr; } @@ -346,7 +351,7 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -396,7 +401,7 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(*qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index a0d5f4bd36..edb9d1adf4 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -44,6 +44,8 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rmw_dds_common/qos.hpp" + #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rosidl_typesupport_introspection_cpp/identifier.hpp" @@ -99,9 +101,12 @@ rmw_create_service( } } + rmw_qos_profile_t adapted_qos_policies = + rmw_dds_common::qos_profile_update_best_available_for_services(*qos_policies); + ///// // Check RMW QoS - if (!is_valid_qos(*qos_policies)) { + if (!is_valid_qos(adapted_qos_policies)) { RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); return nullptr; } @@ -155,9 +160,9 @@ rmw_create_service( untyped_response_members, type_support->typesupport_identifier); std::string response_topic_name = _create_topic_name( - qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + &adapted_qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); std::string request_topic_name = _create_topic_name( - qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + &adapted_qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); // Get request topic and type eprosima::fastdds::dds::TypeSupport request_fastdds_type; @@ -291,7 +296,7 @@ rmw_create_service( // Create and register Topics // Same default topic QoS for both topics eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) { + if (!get_topic_qos(adapted_qos_policies, topic_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); return nullptr; } @@ -345,7 +350,7 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -399,7 +404,7 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(*qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; }