From dbb4c354d6ba2b9614d351521782f851a58a4609 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 25 Mar 2021 08:21:47 -0700 Subject: [PATCH] Add API for checking QoS profile compatibility (#1554) * Add API for checking QoS profile compatibility Depends on https://github.com/ros2/rmw/pull/299 Signed-off-by: Jacob Perron * Refactor as free function Returns a struct containing the compatibility enum value and string for the reason. Updated tests to reflect behavior changes upstream. Signed-off-by: Jacob Perron --- .../include/rclcpp/exceptions/exceptions.hpp | 7 +++ rclcpp/include/rclcpp/qos.hpp | 63 +++++++++++++++++++ rclcpp/src/rclcpp/qos.cpp | 41 ++++++++++++ rclcpp/test/rclcpp/test_qos.cpp | 42 +++++++++++++ 4 files changed, 153 insertions(+) diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index a0c711290c..27c695c0b6 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -303,6 +303,13 @@ class InvalidQosOverridesException : public std::runtime_error using std::runtime_error::runtime_error; }; +/// Thrown if a QoS compatibility check fails. +class QoSCheckCompatibleException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + } // namespace exceptions } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index edc3a58bbe..4160360756 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -18,6 +18,7 @@ #include #include "rclcpp/duration.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/visibility_control.hpp" #include "rcl/logging_rosout.h" #include "rmw/incompatible_qos_events_statuses.h" @@ -62,6 +63,13 @@ enum class LivelinessPolicy Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN, }; +enum class QoSCompatibility +{ + Ok = RMW_QOS_COMPATIBILITY_OK, + Warning = RMW_QOS_COMPATIBILITY_WARNING, + Error = RMW_QOS_COMPATIBILITY_ERROR, +}; + /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. struct RCLCPP_PUBLIC QoSInitialization { @@ -273,6 +281,61 @@ bool operator==(const QoS & left, const QoS & right); RCLCPP_PUBLIC bool operator!=(const QoS & left, const QoS & right); +/// Result type for checking QoS compatibility +/** + * \see rclcpp::qos_check_compatible() + */ +struct QoSCheckCompatibleResult +{ + /// Compatibility result. + QoSCompatibility compatibility; + + /// Reason for a (possible) incompatibility. + /** + * Set if compatiblity is QoSCompatibility::Warning or QoSCompatiblity::Error. + * Not set if the QoS profiles are compatible. + */ + std::string reason; +}; + +/// Check if two QoS profiles are compatible. +/** + * Two QoS profiles are compatible if a publisher and subcription + * using the QoS policies can communicate with each other. + * + * If any policies have value "system default" or "unknown" then it is possible that + * compatiblity cannot be determined. + * In this case, the value QoSCompatility::Warning is set as part of + * the returned structure. + * + * Example usage: + * + * ```cpp + * rclcpp::QoSCheckCompatibleResult result = rclcpp::qos_check_compatible( + * publisher_qos, subscription_qos); + * if (rclcpp::QoSCompatibility::Error != result.compatibility) { + * // QoS not compatible ... + * // result.reason contains info about the incompatibility + * } else if (rclcpp::QoSCompatibility::Warning != result.compatibility) { + * // QoS may not be compatible ... + * // result.reason contains info about the possible incompatibility + * } + * ``` + * + * \param[in] publisher_qos: The QoS profile for a publisher. + * \param[in] subscription_qos: The QoS profile for a subscription. + * \return Struct with compatiblity set to QoSCompatibility::Ok if the QoS profiles are + * compatible, or + * \return Struct with compatibility set to QoSCompatibility::Warning if there is a chance + * the QoS profiles are not compatible, or + * \return Struct with compatibility set to QoSCompatibility::Error if the QoS profiles are + * not compatible. + * \throws rclcpp::exceptions::QoSCheckCompatibilityException if an unexpected error occurs. + */ +RCLCPP_PUBLIC +QoSCheckCompatibleResult +qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos); + /** * Clock QoS class * - History: Keep last, diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index d0f47cd01e..8b912de07f 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -16,7 +16,9 @@ #include +#include "rmw/error_handling.h" #include "rmw/types.h" +#include "rmw/qos_profiles.h" namespace rclcpp { @@ -309,6 +311,45 @@ bool operator!=(const QoS & left, const QoS & right) return !(left == right); } +QoSCheckCompatibleResult +qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos) +{ + rmw_qos_compatibility_type_t compatible; + const size_t reason_size = 2048u; + char reason_c_str[reason_size] = ""; + rmw_ret_t ret = rmw_qos_profile_check_compatible( + publisher_qos.get_rmw_qos_profile(), + subscription_qos.get_rmw_qos_profile(), + &compatible, + reason_c_str, + reason_size); + if (RMW_RET_OK != ret) { + std::string error_str(rmw_get_error_string().str); + rmw_reset_error(); + throw rclcpp::exceptions::QoSCheckCompatibleException{error_str}; + } + + QoSCheckCompatibleResult result; + result.reason = std::string(reason_c_str); + + switch (compatible) { + case RMW_QOS_COMPATIBILITY_OK: + result.compatibility = QoSCompatibility::Ok; + break; + case RMW_QOS_COMPATIBILITY_WARNING: + result.compatibility = QoSCompatibility::Warning; + break; + case RMW_QOS_COMPATIBILITY_ERROR: + result.compatibility = QoSCompatibility::Error; + break; + default: + throw rclcpp::exceptions::QoSCheckCompatibleException{ + "Unexpected compatibility value returned by rmw '" + std::to_string(compatible) + + "'"}; + } + return result; +} + ClockQoS::ClockQoS(const QoSInitialization & qos_initialization) // Using `rmw_qos_profile_sensor_data` intentionally. // It's best effort and `qos_initialization` is overriding the depth to 1. diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp index 6786750ed4..47d1d10b07 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -14,6 +14,8 @@ #include +#include + #include "rclcpp/qos.hpp" #include "rmw/types.h" @@ -208,3 +210,43 @@ TEST(TestQoS, policy_name_from_kind) { "LIFESPAN_QOS_POLICY", rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN)); } + +TEST(TestQoS, qos_check_compatible) +{ + // Compatible + { + rclcpp::QoS qos = rclcpp::QoS(1) + .reliable() + .durability_volatile() + .deadline(rclcpp::Duration(1, 0u)) + .lifespan(rclcpp::Duration(1, 0u)) + .liveliness(rclcpp::LivelinessPolicy::Automatic) + .liveliness_lease_duration(rclcpp::Duration(1, 0u)); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(qos, qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_EQ(ret.reason, std::string("")); + } + + // Note, the following incompatible tests assume we are using a DDS middleware, + // and may not be valid for other RMWs. + // TODO(jacobperron): programmatically check if current RMW is one of the officially + // supported DDS middlewares before running the following tests + + // Incompatible + { + rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort(); + rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error); + EXPECT_FALSE(ret.reason.empty()); + } + + // Warn of possible incompatibility + { + rclcpp::SystemDefaultsQoS pub_qos; + rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning); + EXPECT_FALSE(ret.reason.empty()); + } +}