Implement get_actual_qos() for subscriptions (#455)

Signed-off-by: Miaofei <miaofei@amazon.com>

rcl/include/rcl/subscription.h

@@ -445,6 +445,32 @@ rcl_subscription_get_publisher_count(
   const rcl_subscription_t * subscription,
   size_t * publisher_count);
 
+/// Get the actual qos settings of the subscription.
+/**
+ * Used to get the actual qos settings of the subscription.
+ * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT
+ * can only be resolved after the creation of the subscription, and it
+ * depends on the underlying rmw implementation.
+ * If the underlying setting in use can't be represented in ROS terms,
+ * it will be set to RMW_*_UNKNOWN.
+ * The returned struct is only valid as long as the rcl_subscription_t is valid.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] subscription pointer to the rcl subscription
+ * \return qos struct if successful, otherwise `NULL`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const rmw_qos_profile_t *
+rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/subscription.c

@@ -167,6 +167,17 @@ rcl_subscription_init(
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     goto fail;
   }
+  // get actual qos, and store it
+  rmw_ret = rmw_subscription_get_actual_qos(
+    subscription->impl->rmw_handle,
+    &subscription->impl->actual_qos);
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    ret = RCL_RET_ERROR;
+    goto fail;
+  }
+  subscription->impl->actual_qos.avoid_ros_namespace_conventions =
+    options->qos.avoid_ros_namespace_conventions;
   // options
   subscription->impl->options = *options;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized");
@@ -359,6 +370,15 @@ rcl_subscription_get_publisher_count(
   return RCL_RET_OK;
 }
 
+const rmw_qos_profile_t *
+rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription)
+{
+  if (!rcl_subscription_is_valid(subscription)) {
+    return NULL;
+  }
+  return &subscription->impl->actual_qos;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/subscription_impl.h

@@ -22,6 +22,7 @@
 typedef struct rcl_subscription_impl_t
 {
   rcl_subscription_options_t options;
+  rmw_qos_profile_t actual_qos;
   rmw_subscription_t * rmw_handle;
 } rcl_subscription_impl_t;
 

rcl/test/rcl/test_get_actual_qos.cpp

@@ -67,6 +67,34 @@ std::ostream & operator<<(
   return out;
 }
 
+bool operator==(
+  const rmw_time_t & lhs,
+  const rmw_time_t & rhs)
+{
+  return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec;
+}
+
+bool operator>=(
+  const rmw_time_t & lhs,
+  const rmw_time_t & rhs)
+{
+  if (lhs.sec > rhs.sec) {
+    return true;
+  } else if (lhs.sec == rhs.sec) {
+    return lhs.nsec >= rhs.nsec;
+  } else {
+    return false;
+  }
+}
+
+std::ostream & operator<<(
+  std::ostream & out,
+  const rmw_time_t & param)
+{
+  out << "sec: " << param.sec << " nsec: " << param.nsec;
+  return out;
+}
+
 class TEST_FIXTURE_P_RMW (TestGetActualQoS)
   : public ::testing::TestWithParam<TestParameters>
 {
@@ -114,7 +142,12 @@ protected:
   rcl_context_t * context_ptr;
 };
 
-TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) {
+
+class TEST_FIXTURE_P_RMW (TestPublisherGetActualQoS)
+  : public TEST_FIXTURE_P_RMW(TestGetActualQoS) {};
+
+TEST_P_RMW(TestPublisherGetActualQoS, test_publisher_get_qos_settings)
+{
   TestParameters parameters = GetParam();
   std::string topic_name("/test_publisher_get_actual_qos__");
   rcl_ret_t ret;
@@ -130,6 +163,7 @@ TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) {
   const rmw_qos_profile_t * qos;
   qos = rcl_publisher_get_actual_qos(&pub);
   EXPECT_NE(nullptr, qos) << rcl_get_error_string().str;
+
   EXPECT_EQ(
     qos->history,
     parameters.qos_expected.history);
@@ -142,6 +176,18 @@ TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) {
   EXPECT_EQ(
     qos->durability,
     parameters.qos_expected.durability);
+  EXPECT_GE(
+    qos->deadline,
+    parameters.qos_expected.deadline);
+  EXPECT_GE(
+    qos->lifespan,
+    parameters.qos_expected.lifespan);
+  EXPECT_EQ(
+    qos->liveliness,
+    parameters.qos_expected.liveliness);
+  EXPECT_GE(
+    qos->liveliness_lease_duration,
+    parameters.qos_expected.liveliness_lease_duration);
   EXPECT_EQ(
     qos->avoid_ros_namespace_conventions,
     parameters.qos_expected.avoid_ros_namespace_conventions);
@@ -151,56 +197,198 @@ TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) {
   rcl_reset_error();
 }
 
-static constexpr rmw_qos_profile_t non_default_qos_profile()
+
+class TEST_FIXTURE_P_RMW (TestSubscriptionGetActualQoS)
+  : public TEST_FIXTURE_P_RMW(TestGetActualQoS) {};
+
+TEST_P_RMW(TestSubscriptionGetActualQoS, test_subscription_get_qos_settings)
+{
+  TestParameters parameters = GetParam();
+  std::string topic_name("/test_subscription_get_qos_settings");
+  rcl_ret_t ret;
+
+  rcl_subscription_t pub = rcl_get_zero_initialized_subscription();
+  rcl_subscription_options_t pub_ops = rcl_subscription_get_default_options();
+  pub_ops.qos = parameters.qos_to_set;
+  auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes);
+  ret = rcl_subscription_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+
+  const rmw_qos_profile_t * qos;
+  qos = rcl_subscription_get_actual_qos(&pub);
+  EXPECT_NE(nullptr, qos) << rcl_get_error_string().str;
+
+  EXPECT_EQ(
+    qos->history,
+    parameters.qos_expected.history);
+  EXPECT_EQ(
+    qos->depth,
+    parameters.qos_expected.depth);
+  EXPECT_EQ(
+    qos->reliability,
+    parameters.qos_expected.reliability);
+  EXPECT_EQ(
+    qos->durability,
+    parameters.qos_expected.durability);
+  EXPECT_GE(
+    qos->deadline,
+    parameters.qos_expected.deadline);
+  // note: lifespan is not a concept in subscriptions
+  EXPECT_EQ(
+    qos->liveliness,
+    parameters.qos_expected.liveliness);
+  EXPECT_GE(
+    qos->liveliness_lease_duration,
+    parameters.qos_expected.liveliness_lease_duration);
+  EXPECT_EQ(
+    qos->avoid_ros_namespace_conventions,
+    parameters.qos_expected.avoid_ros_namespace_conventions);
+
+  ret = rcl_subscription_fini(&pub, this->node_ptr);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+}
+
+
+//
+// other input profile settings
+//
+
+static constexpr rmw_qos_profile_t
+nondefault_qos_profile()
 {
   rmw_qos_profile_t profile = rmw_qos_profile_default;
   profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
   profile.depth = 1000;
   profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
   profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  profile.deadline.sec = 1;
+  profile.lifespan.nsec = 500000;
   profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  profile.liveliness_lease_duration.sec = 1;
   profile.avoid_ros_namespace_conventions = true;
   return profile;
 }
 
-static constexpr rmw_qos_profile_t expected_fastrtps_default_qos_profile()
+static constexpr rmw_qos_profile_t
+nondefault_qos_profile_for_fastrtps()
+{
+  rmw_qos_profile_t profile = rmw_qos_profile_default;
+  profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
+  profile.depth = 1000;
+  profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  profile.deadline.sec = 1;
+  profile.lifespan.nsec = 500000;
+  profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  // profile.liveliness_lease_duration.sec = 1; // fastrtps does not fully support liveliness
+  profile.avoid_ros_namespace_conventions = true;
+  return profile;
+}
+
+
+//
+// expected output profile settings
+//
+
+static constexpr rmw_qos_profile_t
+expected_default_qos_profile()
+{
+  rmw_qos_profile_t profile = rmw_qos_profile_default;
+  profile.deadline.sec = 2147483647;
+  profile.lifespan.sec = 2147483647;
+  profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  profile.liveliness_lease_duration.sec = 2147483647;
+  return profile;
+}
+
+static constexpr rmw_qos_profile_t
+expected_nondefault_qos_profile()
+{
+  return nondefault_qos_profile();
+}
+
+static constexpr rmw_qos_profile_t
+expected_nondefault_qos_profile_for_fastrtps()
+{
+  rmw_qos_profile_t profile = rmw_qos_profile_default;
+  profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
+  profile.depth = 1000;
+  profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  profile.deadline.sec = 1;
+  profile.lifespan.nsec = 500000;
+  profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  profile.liveliness_lease_duration.sec = 2147483647;
+  profile.avoid_ros_namespace_conventions = true;
+  return profile;
+}
+
+static constexpr rmw_qos_profile_t
+expected_system_default_publisher_qos_profile()
+{
+  rmw_qos_profile_t profile = rmw_qos_profile_default;
+  profile.depth = 1;
+  profile.deadline.sec = 2147483647;
+  profile.lifespan.sec = 2147483647;
+  profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  profile.liveliness_lease_duration.sec = 2147483647;
+  return profile;
+}
+
+static constexpr rmw_qos_profile_t
+expected_system_default_publisher_qos_profile_for_fastrtps()
 {
   rmw_qos_profile_t profile = rmw_qos_profile_default;
   profile.depth = 1;
   profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
   profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  profile.liveliness_lease_duration.sec = 2147483647;
   return profile;
 }
 
-static constexpr rmw_qos_profile_t expected_system_default_qos_profile()
+static constexpr rmw_qos_profile_t
+expected_system_default_subscription_qos_profile()
 {
   rmw_qos_profile_t profile = rmw_qos_profile_default;
   profile.depth = 1;
+  profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  profile.deadline.sec = 2147483647;
   profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  profile.liveliness_lease_duration.sec = 2147483647;
   return profile;
 }
 
-std::vector<TestParameters>
+static constexpr rmw_qos_profile_t
-get_parameters()
+expected_system_default_subscription_qos_profile_for_fastrtps()
 {
-  std::vector<TestParameters>
+  rmw_qos_profile_t profile = rmw_qos_profile_default;
-  parameters({
+  profile.depth = 1;
-    /*
+  profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
-      Testing with default qos settings.
+  profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
-     */
+  profile.deadline.sec = 2147483647;
-    {
+  profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
-      rmw_qos_profile_default,
+  profile.liveliness_lease_duration.sec = 2147483647;
-      rmw_qos_profile_default,
+  return profile;
-      "publisher_default_qos"
+}
-    },
+
-    /*
+
-      Test with non-default settings.
+//
-     */
+// create set of input and expected output profile setting pairs
-    {
+//
-      non_default_qos_profile(),
+std::vector<TestParameters>
-      non_default_qos_profile(),
+get_parameters(bool for_publisher)
-      "publisher_non_default_qos"
+{
-    }
+  std::vector<TestParameters> parameters;
+
+  /*
+   * Testing with default qos settings.
+   */
+  parameters.push_back({
+    rmw_qos_profile_default,
+    expected_default_qos_profile(),
+    "default_qos"
   });
 
 #ifdef RMW_IMPLEMENTATION_STR
@@ -208,21 +396,61 @@ get_parameters()
   if (rmw_implementation_str == "rmw_fastrtps_cpp" ||
     rmw_implementation_str == "rmw_fastrtps_dynamic_cpp")
   {
-    rmw_qos_profile_t expected_system_default_qos = expected_fastrtps_default_qos_profile();
+    /*
+     * Test with non-default settings.
+     */
     parameters.push_back({
-      rmw_qos_profile_system_default,
+      nondefault_qos_profile_for_fastrtps(),
-      expected_system_default_qos,
+      expected_nondefault_qos_profile_for_fastrtps(),
-      "publisher_system_default_qos"});
+      "nondefault_qos"
+    });
+
+    /*
+     * Test with system default settings.
+     */
+    if (for_publisher) {
+      parameters.push_back({
+        rmw_qos_profile_system_default,
+        expected_system_default_publisher_qos_profile_for_fastrtps(),
+        "system_default_publisher_qos"
+      });
+    } else {
+      parameters.push_back({
+        rmw_qos_profile_system_default,
+        expected_system_default_subscription_qos_profile_for_fastrtps(),
+        "system_default_publisher_qos"
+      });
+    }
   } else {
     if (rmw_implementation_str == "rmw_connext_cpp" ||
       rmw_implementation_str == "rmw_connext_dynamic_cpp" ||
       rmw_implementation_str == "rmw_opensplice_cpp")
     {
-      rmw_qos_profile_t expected_system_default_qos = expected_system_default_qos_profile();
+      /*
+       * Test with non-default settings.
+       */
       parameters.push_back({
-        rmw_qos_profile_system_default,
+        nondefault_qos_profile(),
-        expected_system_default_qos,
+        expected_nondefault_qos_profile(),
-        "publisher_system_default_qos"});
+        "nondefault_qos"
+      });
+
+      /*
+       * Test with system default settings.
+       */
+      if (for_publisher) {
+        parameters.push_back({
+          rmw_qos_profile_system_default,
+          expected_system_default_publisher_qos_profile(),
+          "system_default_publisher_qos"
+        });
+      } else {
+        parameters.push_back({
+          rmw_qos_profile_system_default,
+          expected_system_default_subscription_qos_profile(),
+          "system_default_publisher_qos"
+        });
+      }
     }
   }
 #endif
@@ -231,7 +459,13 @@ get_parameters()
 }
 
 INSTANTIATE_TEST_CASE_P_RMW(
-  TestWithDifferentQoSSettings,
+  TestPublisherWithDifferentQoSSettings,
-  TestGetActualQoS,
+  TestPublisherGetActualQoS,
-  ::testing::ValuesIn(get_parameters()),
+  ::testing::ValuesIn(get_parameters(true)),
+  ::testing::PrintToStringParamName());
+
+INSTANTIATE_TEST_CASE_P_RMW(
+  TestSubscriptionWithDifferentQoSSettings,
+  TestSubscriptionGetActualQoS,
+  ::testing::ValuesIn(get_parameters(false)),
   ::testing::PrintToStringParamName());