Add client/service QoS getters (#1784)

mauropasse · Mauro Passerino · web-flow · commit 8b9cabfb9d41 · 2022-03-16T18:05:58.000Z
Signed-off-by: Mauro Passerino &lt;mpasserino@irobot.com&gt;

Co-authored-by: Mauro Passerino &lt;mpasserino@irobot.com&gt;
diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
@@ -40,6 +40,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -218,6 +219,38 @@ class ClientBase
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Get the actual request publsher QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the client, 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_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request publsher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_publisher_actual_qos() const;
+
+  /// Get the actual response subscription QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the client, 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_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_subscription_actual_qos() const;
+
   /// Set a callback to be called when each new response is received.
   /**
    * The callback receives a size_t which is the number of responses received
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
@@ -39,6 +39,7 @@
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -127,6 +128,38 @@ class ServiceBase
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Get the actual response publisher QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the service, 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_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response publisher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_publisher_actual_qos() const;
+
+  /// Get the actual request subscription QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the service, 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_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_subscription_actual_qos() const;
+
   /// Set a callback to be called when each new request is received.
   /**
    * The callback receives a size_t which is the number of requests received
diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp
@@ -201,6 +201,44 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state)
   return in_use_by_wait_set_.exchange(in_use_state);
 }
 
+rclcpp::QoS
+ClientBase::get_request_publisher_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_client_request_publisher_get_actual_qos(client_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get client's request publisher qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS request_publisher_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return request_publisher_qos;
+}
+
+rclcpp::QoS
+ClientBase::get_response_subscription_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_client_response_subscription_get_actual_qos(client_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get client's response subscription qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS response_subscription_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return response_subscription_qos;
+}
+
 void
 ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data)
 {
diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp
@@ -88,6 +88,44 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state)
   return in_use_by_wait_set_.exchange(in_use_state);
 }
 
+rclcpp::QoS
+ServiceBase::get_response_publisher_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_service_response_publisher_get_actual_qos(service_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get service's response publisher qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS response_publisher_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return response_publisher_qos;
+}
+
+rclcpp::QoS
+ServiceBase::get_request_subscription_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_service_request_subscription_get_actual_qos(service_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get service's request subscription qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS request_subscription_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return request_subscription_qos;
+}
+
 void
 ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data)
 {
diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp
@@ -24,6 +24,7 @@
 #include "rcl_interfaces/srv/list_parameters.hpp"
 
 #include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
 
 #include "test_msgs/srv/empty.hpp"
 
@@ -431,3 +432,105 @@ TEST_F(TestClient, on_new_response_callback) {
   std::function<void(size_t)> invalid_cb = nullptr;
   EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument);
 }
+
+TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr);
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  RCLCPP_EXPECT_THROW_EQ(
+    client->get_request_publisher_actual_qos(),
+    std::runtime_error("failed to get client's request publisher qos settings: error not set"));
+}
+
+TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr);
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  RCLCPP_EXPECT_THROW_EQ(
+    client->get_response_subscription_actual_qos(),
+    std::runtime_error("failed to get client's response subscription qos settings: error not set"));
+}
+
+TEST_F(TestClient, client_qos) {
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default;
+  qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+  uint64_t duration = 1;
+  qos_profile.deadline = {duration, duration};
+  qos_profile.lifespan = {duration, duration};
+  qos_profile.liveliness_lease_duration = {duration, duration};
+
+  auto client =
+    node->create_client<test_msgs::srv::Empty>("client", qos_profile);
+
+  auto init_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile);
+  auto rp_qos = client->get_request_publisher_actual_qos();
+  auto rs_qos = client->get_response_subscription_actual_qos();
+
+  EXPECT_EQ(init_qos, rp_qos);
+  // Lifespan has no meaning for subscription/readers
+  rs_qos.lifespan(qos_profile.lifespan);
+  EXPECT_EQ(init_qos, rs_qos);
+}
+
+TEST_F(TestClient, client_qos_depth) {
+  using namespace std::literals::chrono_literals;
+
+  rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default;
+  client_qos_profile.depth = 2;
+
+  auto client = node->create_client<test_msgs::srv::Empty>("test_qos_depth", client_qos_profile);
+
+  uint64_t server_cb_count_ = 0;
+  auto server_callback = [&](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;};
+
+  auto server_node = std::make_shared<rclcpp::Node>("server_node", "/ns");
+
+  rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default;
+
+  auto server = server_node->create_service<test_msgs::srv::Empty>(
+    "test_qos_depth", std::move(server_callback), server_qos_profile);
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  ::testing::AssertionResult request_result = ::testing::AssertionSuccess();
+
+  using SharedFuture = rclcpp::Client<test_msgs::srv::Empty>::SharedFuture;
+  uint64_t client_cb_count_ = 0;
+  auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) {
+      if (nullptr == future_response.get()) {
+        request_result = ::testing::AssertionFailure() << "Future response was null";
+      }
+      client_cb_count_++;
+    };
+
+  uint64_t client_requests = 5;
+  for (uint64_t i = 0; i < client_requests; i++) {
+    client->async_send_request(request, client_callback);
+    std::this_thread::sleep_for(10ms);
+  }
+
+  auto start = std::chrono::steady_clock::now();
+  while ((server_cb_count_ < client_requests) &&
+    (std::chrono::steady_clock::now() - start) < 2s)
+  {
+    rclcpp::spin_some(server_node);
+    std::this_thread::sleep_for(2ms);
+  }
+
+  EXPECT_GT(server_cb_count_, client_qos_profile.depth);
+
+  start = std::chrono::steady_clock::now();
+  while ((client_cb_count_ < client_qos_profile.depth) &&
+    (std::chrono::steady_clock::now() - start) < 1s)
+  {
+    rclcpp::spin_some(node);
+  }
+
+  // Spin an extra time to check if client QoS depth has been ignored,
+  // so more client callbacks might be called than expected.
+  rclcpp::spin_some(node);
+
+  EXPECT_EQ(client_cb_count_, client_qos_profile.depth);
+}
diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp
