Add API for checking QoS profile compatibility

Depends on ros2/rmw#299

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

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -289,6 +289,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 QoSProfileCompatibility
+{
+  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
 {
@@ -140,6 +148,31 @@ class RCLCPP_PUBLIC QoS
   const rmw_qos_profile_t &
   get_rmw_qos_profile() const;
 
+  /// Check if this QoS profile is compatible with another.
+  /**
+   * Two QoS profiles are compatible if a publisher and subcription
+   * using the QoS policies can communicate with each other.
+   *
+   * This QoS profile is assumed to be for a subscription.
+   *
+   * If there is an unknown policy value in either profile, then an exception is thrown.
+   *
+   * \param[in] publisher_qos: The QoS profile for a publisher.
+   * \param[out] reason: An explanation for any incompatibility.
+   *   Set if the return value is QoSProfileCompatibility::Warning or
+   *   QoSProfileCompatibility::Error.
+   *   Not set if the QoS profiles are compatible.
+   *   This parameter is optional.
+   * \return QoSProfileCompatibility::Ok if the QoS profiles are compatible, or
+   * \return QoSProfileCompatibility::Warning if there is a chance the QoS profiles are not
+   *   compatible (this can happen if some policies are set ot "system default"), or
+   * \return QoSProfileCompatibility::Error if the QoS profiles are not compatible.
+   * \throws rclcpp::exceptions::QoSCheckCompatibilityException if any policy value is "unknown",
+   *   or if an unexpected error occurs.
+   */
+  QoSProfileCompatibility
+  compatible(const QoS & publisher_qos, std::string * reason = nullptr);
+
   /// Set the history policy.
   QoS &
   history(HistoryPolicy history);

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
 {
@@ -92,6 +94,47 @@ QoS::get_rmw_qos_profile() const
   return rmw_qos_profile_;
 }
 
+QoSProfileCompatibility
+QoS::compatible(const QoS & publisher_qos, std::string * reason)
+{
+  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(),
+    this->rmw_qos_profile_,
+    &compatible,
+    reason_c_str,
+    reason_size);
+  if (RMW_RET_OK != ret) {
+    auto error_str = rmw_get_error_string().str;
+    rmw_reset_error();
+    throw rclcpp::exceptions::QoSCheckCompatibleException{error_str};
+  }
+
+  if (reason) {
+    *reason = std::string(reason_c_str);
+  }
+
+  QoSProfileCompatibility compatibility;
+  switch (compatible) {
+    case RMW_QOS_COMPATIBILITY_OK:
+      compatibility = QoSProfileCompatibility::Ok;
+      break;
+    case RMW_QOS_COMPATIBILITY_WARNING:
+      compatibility = QoSProfileCompatibility::Warning;
+      break;
+    case RMW_QOS_COMPATIBILITY_ERROR:
+      compatibility = QoSProfileCompatibility::Error;
+      break;
+    default:
+      throw rclcpp::exceptions::QoSCheckCompatibleException{
+              "Unexpected compatibility value returned by rmw '" + std::to_string(compatible) +
+              "'"};
+  }
+  return compatibility;
+}
+
 QoS &
 QoS::history(rmw_qos_history_policy_t history)
 {

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,77 @@ TEST(TestQoS, policy_name_from_kind) {
     "LIFESPAN_QOS_POLICY",
     rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN));
 }
+
+TEST(TestQoS, compatible)
+{
+  // Compatible
+  {
+    rclcpp::SensorDataQoS pub_qos;
+    rclcpp::SensorDataQoS sub_qos;
+    rclcpp::QoSProfileCompatibility ret = sub_qos.compatible(pub_qos);
+    EXPECT_EQ(ret, rclcpp::QoSProfileCompatibility::Ok);
+  }
+
+  // Compatible with reason
+  {
+    rclcpp::SensorDataQoS pub_qos;
+    rclcpp::SensorDataQoS sub_qos;
+    std::string reason;
+    rclcpp::QoSProfileCompatibility ret = sub_qos.compatible(pub_qos, &reason);
+    EXPECT_EQ(ret, rclcpp::QoSProfileCompatibility::Ok);
+    EXPECT_TRUE(reason.empty());
+  }
+
+  // Error on "unknown" in pub
+  {
+    rclcpp::QoS pub_qos(rclcpp::KeepLast(1), rmw_qos_profile_unknown);
+    rclcpp::QoS sub_qos(1);
+    EXPECT_THROW(sub_qos.compatible(pub_qos), rclcpp::exceptions::QoSCheckCompatibleException);
+  }
+
+  // Error on "unknown" in sub
+  {
+    rclcpp::QoS pub_qos(1);
+    rclcpp::QoS sub_qos(rclcpp::KeepLast(1), rmw_qos_profile_unknown);
+    EXPECT_THROW(sub_qos.compatible(pub_qos), rclcpp::exceptions::QoSCheckCompatibleException);
+  }
+
+  // Note, the following incompatible tests assume we are using a DDS middleware,
+  // and may not be valid for other RMWs.
+
+  // Incompatible
+  {
+    rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort();
+    rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable();
+    rclcpp::QoSProfileCompatibility ret = sub_qos.compatible(pub_qos);
+    EXPECT_EQ(ret, rclcpp::QoSProfileCompatibility::Error);
+  }
+
+  // Incompatible with reason
+  {
+    rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort();
+    rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable();
+    std::string reason;
+    rclcpp::QoSProfileCompatibility ret = sub_qos.compatible(pub_qos, &reason);
+    EXPECT_EQ(ret, rclcpp::QoSProfileCompatibility::Error);
+    EXPECT_FALSE(reason.empty());
+  }
+
+  // Warn of possible incompatibility
+  {
+    rclcpp::SystemDefaultsQoS pub_qos;
+    rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable();
+    rclcpp::QoSProfileCompatibility ret = sub_qos.compatible(pub_qos);
+    EXPECT_EQ(ret, rclcpp::QoSProfileCompatibility::Warning);
+  }
+
+  // Warn of possible incompatibility with reason
+  {
+    rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort();
+    rclcpp::SystemDefaultsQoS sub_qos;
+    std::string reason;
+    rclcpp::QoSProfileCompatibility ret = sub_qos.compatible(pub_qos, &reason);
+    EXPECT_EQ(ret, rclcpp::QoSProfileCompatibility::Warning);
+    EXPECT_FALSE(reason.empty());
+  }
+}