Add API for checking QoS profile compatibility (#1554)

* Add API for checking QoS profile compatibility

Depends on ros2/rmw#299

Signed-off-by: Jacob Perron <[email protected]>

* 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 <[email protected]>

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
 

rclcpp/include/rclcpp/qos.hpp

@@ -18,6 +18,7 @@
 #include <string>
 
 #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,

rclcpp/src/rclcpp/qos.cpp

@@ -16,7 +16,9 @@
 
 #include <string>
 
+#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.

rclcpp/test/rclcpp/test_qos.cpp

@@ -14,6 +14,8 @@
 
 #include <gtest/gtest.h>
 
+#include <string>
+
 #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());
+  }
+}