Add function to get publisher actual qos settings (#406)

* Add rcl_publisher_get_actual_qos function

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

* Add test for rcl_publisher_get_actual_qos

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

* Correct RMW_IMPLEMENTATION checking. Add opensplice system default qos test

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

* Add system default test for connext

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

* Solve windows build problem

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>

rcl/include/rcl/publisher.h

@@ -455,7 +455,7 @@ rcl_publisher_is_valid_except_context(const rcl_publisher_t * publisher);
  * \param[out] subscription_count number of matched subscriptions
  * \return `RCL_RET_OK` if the count was retrieved, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
- * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or
+ * \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or
  * \return `RCL_RET_ERROR` if an unspecified error occurs.
  */
 RCL_PUBLIC
@@ -465,6 +465,32 @@ rcl_publisher_get_subscription_count(
   const rcl_publisher_t * publisher,
   size_t * subscription_count);
 
+/// Get the actual qos settings of the publisher.
+/**
+ * Used to get the actual qos settings of the publisher.
+ * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT
+ * can only be resolved after the creation of the publisher, 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_publisher_t is valid.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] publisher pointer to the rcl publisher
+ * \return qos struct if successful, otherwise `NULL`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const rmw_qos_profile_t *
+rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/publisher.c

@@ -35,6 +35,7 @@ extern "C"
 typedef struct rcl_publisher_impl_t
 {
   rcl_publisher_options_t options;
+  rmw_qos_profile_t actual_qos;
   rcl_context_t * context;
   rmw_publisher_t * rmw_handle;
 } rcl_publisher_impl_t;
@@ -171,6 +172,17 @@ rcl_publisher_init(
     &(options->qos));
   RCL_CHECK_FOR_NULL_WITH_MSG(publisher->impl->rmw_handle,
     rmw_get_error_string().str, goto fail);
+  // get actual qos, and store it
+  rmw_ret = rmw_publisher_get_actual_qos(
+    publisher->impl->rmw_handle,
+    &publisher->impl->actual_qos);
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    ret = RCL_RET_ERROR;
+    goto fail;
+  }
+  publisher->impl->actual_qos.avoid_ros_namespace_conventions =
+    options->qos.avoid_ros_namespace_conventions;
   // options
   publisher->impl->options = *options;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized");
@@ -349,6 +361,15 @@ rcl_publisher_get_subscription_count(
   return RCL_RET_OK;
 }
 
+const rmw_qos_profile_t *
+rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher)
+{
+  if (!rcl_publisher_is_valid_except_context(publisher)) {
+    return NULL;
+  }
+  return &publisher->impl->actual_qos;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/test/CMakeLists.txt

@@ -119,6 +119,14 @@ function(test_target_function)
     AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
   )
 
+  rcl_add_custom_gtest(test_get_actual_qos${target_suffix}
+    SRCS rcl/test_get_actual_qos.cpp
+    ENV ${rmw_implementation_env_var}
+    APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+    LIBRARIES ${PROJECT_NAME}
+    AMENT_DEPENDENCIES ${rmw_implementation} "test_msgs"
+  )
+
   rcl_add_custom_gtest(test_init${target_suffix}
     SRCS rcl/test_init.cpp
     ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var}

rcl/test/rcl/test_get_actual_qos.cpp

@@ -0,0 +1,223 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/rcl.h"
+#include "rcl/publisher.h"
+
+#include "rcutils/logging_macros.h"
+#include "rcutils/macros.h"
+
+#include "test_msgs/msg/primitives.h"
+#include "test_msgs/srv/primitives.h"
+
+#ifdef RMW_IMPLEMENTATION
+# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
+# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
+# define RMW_IMPLEMENTATION_STR RCUTILS_STRINGIFY(RMW_IMPLEMENTATION)
+#else
+# define CLASSNAME(NAME, SUFFIX) NAME
+#endif
+
+#define EXPAND(x) x
+#define TEST_FIXTURE_P_RMW(test_fixture_name) CLASSNAME(test_fixture_name, \
+    RMW_IMPLEMENTATION)
+#define APPLY(macro, ...) EXPAND(macro(__VA_ARGS__))
+#define TEST_P_RMW(test_case_name, test_name) \
+  APPLY(TEST_P, \
+    CLASSNAME(test_case_name, RMW_IMPLEMENTATION), test_name)
+#define INSTANTIATE_TEST_CASE_P_RMW(instance_name, test_case_name, ...) \
+  EXPAND(APPLY(INSTANTIATE_TEST_CASE_P, instance_name, \
+    CLASSNAME(test_case_name, RMW_IMPLEMENTATION), __VA_ARGS__))
+
+/**
+ * Parameterized test.
+ * The first param are the NodeOptions used to create the nodes.
+ * The second param are the expect intraprocess count results.
+ */
+struct TestParameters
+{
+  rmw_qos_profile_t qos_to_set;
+  rmw_qos_profile_t qos_expected;
+  std::string description;
+};
+
+std::ostream & operator<<(
+  std::ostream & out,
+  const TestParameters & params)
+{
+  out << params.description;
+  return out;
+}
+
+class TEST_FIXTURE_P_RMW (TestGetActualQoS)
+  : public ::testing::TestWithParam<TestParameters>
+{
+public:
+  rcl_node_t * node_ptr;
+  rcl_context_t * context_ptr;
+  void SetUp()
+  {
+    rcl_ret_t ret;
+    rcl_node_options_t node_options = rcl_node_get_default_options();
+
+    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+    ret = rcl_init_options_init(&init_options, rcl_get_default_allocator());
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    this->context_ptr = new rcl_context_t;
+    *this->context_ptr = rcl_get_zero_initialized_context();
+    ret = rcl_init(0, nullptr, &init_options, this->context_ptr);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    this->node_ptr = new rcl_node_t;
+    *this->node_ptr = rcl_get_zero_initialized_node();
+    const char * name = "test_get_actual_qos_node";
+    ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  }
+
+  void TearDown()
+  {
+    rcl_ret_t ret;
+
+    ret = rcl_node_fini(this->node_ptr);
+    delete this->node_ptr;
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+    ret = rcl_shutdown(this->context_ptr);
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+    ret = rcl_context_fini(this->context_ptr);
+    delete this->context_ptr;
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  }
+};
+
+TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) {
+  TestParameters parameters = GetParam();
+  std::string topic_name("/test_publisher_get_actual_qos__");
+  rcl_ret_t ret;
+
+  rcl_publisher_t pub = rcl_get_zero_initialized_publisher();
+  rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options();
+  pub_ops.qos = parameters.qos_to_set;
+  auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives);
+  ret = rcl_publisher_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_publisher_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_EQ(
+    qos->avoid_ros_namespace_conventions,
+    parameters.qos_expected.avoid_ros_namespace_conventions);
+
+  ret = rcl_publisher_fini(&pub, this->node_ptr);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  rcl_reset_error();
+}
+
+std::vector<TestParameters>
+get_parameters()
+{
+  std::vector<TestParameters>
+  parameters({
+    /*
+      Testing with default qos settings.
+     */
+    {
+      rmw_qos_profile_default,
+      rmw_qos_profile_default,
+      "publisher_default_qos"
+    },
+    /*
+      Test with non-default settings.
+     */
+    {
+      {
+        RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+        1000,
+        RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+        RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+        true
+      },
+      {
+        RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+        1000,
+        RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+        RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+        true
+      },
+      "publisher_non_default_qos"
+    }
+  });
+
+#ifdef RMW_IMPLEMENTATION_STR
+  std::string rmw_implementation_str = RMW_IMPLEMENTATION_STR;
+  if (!rmw_implementation_str.compare("rmw_fastrtps_cpp") ||
+    !rmw_implementation_str.compare("rmw_fastrtps_dynamic_cpp"))
+  {
+    rmw_qos_profile_t expected_system_default_qos = {
+      RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+      1,
+      RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+      RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+      false};
+    parameters.push_back({
+      rmw_qos_profile_system_default,
+      expected_system_default_qos,
+      "publisher_system_default_qos"});
+  } else {
+    if (!rmw_implementation_str.compare("rmw_opensplice_cpp") ||
+      !rmw_implementation_str.compare("rmw_connext_cpp") ||
+      !rmw_implementation_str.compare("rmw_connext_dynamic_cpp"))
+    {
+      rmw_qos_profile_t expected_system_default_qos = {
+        RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+        1,
+        RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+        RMW_QOS_POLICY_DURABILITY_VOLATILE,
+        false};
+      parameters.push_back({
+        rmw_qos_profile_system_default,
+        expected_system_default_qos,
+        "publisher_system_default_qos"});
+    }
+  }
+#endif
+  return parameters;
+}
+
+INSTANTIATE_TEST_CASE_P_RMW(
+  TestWithDifferentQoSSettings,
+  TestGetActualQoS,
+  ::testing::ValuesIn(get_parameters()),
+  ::testing::PrintToStringParamName());