diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h
index 567c782..bb45ce8 100644
--- a/rcl/include/rcl/subscription.h
+++ b/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.
+ *
+ *
+ * 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
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
index 1cda902..b881502 100644
--- a/rcl/src/rcl/subscription.c
+++ b/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
diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h
index 7ad0fd8..00756f2 100644
--- a/rcl/src/rcl/subscription_impl.h
+++ b/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;
diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp
index c8570f4..be4f485 100644
--- a/rcl/test/rcl/test_get_actual_qos.cpp
+++ b/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
{
@@ -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
-get_parameters()
+static constexpr rmw_qos_profile_t
+expected_system_default_subscription_qos_profile_for_fastrtps()
{
- std::vector
- parameters({
- /*
- Testing with default qos settings.
- */
- {
- rmw_qos_profile_default,
- rmw_qos_profile_default,
- "publisher_default_qos"
- },
- /*
- Test with non-default settings.
- */
- {
- non_default_qos_profile(),
- non_default_qos_profile(),
- "publisher_non_default_qos"
- }
+ rmw_qos_profile_t profile = rmw_qos_profile_default;
+ profile.depth = 1;
+ profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+ profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+ profile.deadline.sec = 2147483647;
+ profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+ profile.liveliness_lease_duration.sec = 2147483647;
+ return profile;
+}
+
+
+//
+// create set of input and expected output profile setting pairs
+//
+std::vector
+get_parameters(bool for_publisher)
+{
+ std::vector 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,
- expected_system_default_qos,
- "publisher_system_default_qos"});
+ nondefault_qos_profile_for_fastrtps(),
+ expected_nondefault_qos_profile_for_fastrtps(),
+ "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,
- expected_system_default_qos,
- "publisher_system_default_qos"});
+ nondefault_qos_profile(),
+ expected_nondefault_qos_profile(),
+ "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,
- TestGetActualQoS,
- ::testing::ValuesIn(get_parameters()),
+ TestPublisherWithDifferentQoSSettings,
+ TestPublisherGetActualQoS,
+ ::testing::ValuesIn(get_parameters(true)),
+ ::testing::PrintToStringParamName());
+
+INSTANTIATE_TEST_CASE_P_RMW(
+ TestSubscriptionWithDifferentQoSSettings,
+ TestSubscriptionGetActualQoS,
+ ::testing::ValuesIn(get_parameters(false)),
::testing::PrintToStringParamName());