diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 693390a34..0b886e7e3 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -216,6 +216,15 @@ rmw_subscription_count_matched_publishers( subscription, publisher_count); } +rmw_ret_t +rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos( + subscription, qos); +} + rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index d397b4b9e..d3dfc3e25 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -212,6 +212,15 @@ rmw_subscription_count_matched_publishers( subscription, publisher_count); } +rmw_ret_t +rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos( + subscription, qos); +} + rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index fce19d99f..a0cce9200 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -28,6 +28,10 @@ class PublisherAttributes; } // namespace fastrtps } // namespace eprosima +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +is_valid_qos(const rmw_qos_profile_t & qos_policies); + RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( @@ -40,8 +44,22 @@ get_datawriter_qos( const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr); -RMW_FASTRTPS_SHARED_CPP_PUBLIC -bool -is_valid_qos(const rmw_qos_profile_t & qos_policies); +template +void +dds_qos_to_rmw_qos( + const AttributeT & dds_qos, + rmw_qos_profile_t * qos); + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +dds_qos_to_rmw_qos( + const eprosima::fastrtps::PublisherAttributes & dds_qos, + rmw_qos_profile_t * qos); + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +dds_qos_to_rmw_qos( + const eprosima::fastrtps::SubscriberAttributes & dds_qos, + rmw_qos_profile_t * qos); #endif // RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 9dfcdc689..8ffc8cd02 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -258,6 +258,12 @@ __rmw_subscription_count_matched_publishers( const rmw_subscription_t * subscription, size_t * publisher_count); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos); + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_take( diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 52d7c7b52..c3a604305 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -169,3 +169,80 @@ is_valid_qos(const rmw_qos_profile_t & qos_policies) } return true; } + +template +void +dds_qos_to_rmw_qos( + const AttributeT & dds_qos, + rmw_qos_profile_t * qos) +{ + switch (dds_qos.topic.historyQos.kind) { + case eprosima::fastrtps::KEEP_LAST_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + break; + case eprosima::fastrtps::KEEP_ALL_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + break; + default: + qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; + break; + } + qos->depth = static_cast(dds_qos.topic.historyQos.depth); + + switch (dds_qos.qos.m_reliability.kind) { + case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + default: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + break; + } + + switch (dds_qos.qos.m_durability.kind) { + case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + default: + qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + + qos->deadline.sec = dds_qos.qos.m_deadline.period.seconds; + qos->deadline.nsec = dds_qos.qos.m_deadline.period.nanosec; + + qos->lifespan.sec = dds_qos.qos.m_lifespan.duration.seconds; + qos->lifespan.nsec = dds_qos.qos.m_lifespan.duration.nanosec; + + switch (dds_qos.qos.m_liveliness.kind) { + case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case eprosima::fastrtps::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + break; + case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + break; + default: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + break; + } + qos->liveliness_lease_duration.sec = dds_qos.qos.m_liveliness.lease_duration.seconds; + qos->liveliness_lease_duration.nsec = dds_qos.qos.m_liveliness.lease_duration.nanosec; +} + +template +void dds_qos_to_rmw_qos( + const eprosima::fastrtps::PublisherAttributes & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_qos_to_rmw_qos( + const eprosima::fastrtps::SubscriberAttributes & dds_qos, + rmw_qos_profile_t * qos); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 5fe9c2c79..0186ce214 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -146,65 +146,7 @@ __rmw_publisher_get_actual_qos( const eprosima::fastrtps::PublisherAttributes & attributes = fastrtps_pub->getAttributes(); - switch (attributes.topic.historyQos.kind) { - case eprosima::fastrtps::KEEP_LAST_HISTORY_QOS: - qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - break; - case eprosima::fastrtps::KEEP_ALL_HISTORY_QOS: - qos->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - break; - default: - qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - break; - } - qos->depth = static_cast(attributes.topic.historyQos.depth); - - switch (attributes.qos.m_reliability.kind) { - case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: - qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - break; - case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: - qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - break; - default: - qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - break; - } - - switch (attributes.qos.m_durability.kind) { - case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: - qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - break; - case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: - qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; - break; - default: - qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - break; - } - - qos->deadline.sec = attributes.qos.m_deadline.period.seconds; - qos->deadline.nsec = attributes.qos.m_deadline.period.nanosec; - - qos->lifespan.sec = attributes.qos.m_lifespan.duration.seconds; - qos->lifespan.nsec = attributes.qos.m_lifespan.duration.nanosec; - - switch (attributes.qos.m_liveliness.kind) { - case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - break; - case eprosima::fastrtps::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; - break; - case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - break; - default: - qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - break; - } - qos->liveliness_lease_duration.sec = attributes.qos.m_liveliness.lease_duration.seconds; - qos->liveliness_lease_duration.nsec = attributes.qos.m_liveliness.lease_duration.nanosec; + dds_qos_to_rmw_qos(attributes, qos); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 9ecb309cf..429acfd05 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -106,4 +106,27 @@ __rmw_subscription_count_matched_publishers( return RMW_RET_OK; } +rmw_ret_t +__rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(subscription->data); + if (info == nullptr) { + return RMW_RET_ERROR; + } + eprosima::fastrtps::Subscriber * fastrtps_sub = info->subscriber_; + if (fastrtps_sub == nullptr) { + return RMW_RET_ERROR; + } + const eprosima::fastrtps::SubscriberAttributes & attributes = + fastrtps_sub->getAttributes(); + + dds_qos_to_rmw_qos(attributes, qos); + + return RMW_RET_OK; +} } // namespace rmw_fastrtps_shared_cpp