From 62a3edf84bb1d157c56f51a9f36ecf09a86ac797 Mon Sep 17 00:00:00 2001
From: Mauro Passerino <mpasserino@irobot.com>
Date: Fri, 17 Sep 2021 19:02:08 +0100
Subject: [PATCH] Add apis to retrieve service/client QoS

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>
---
 rmw_fastrtps_cpp/src/rmw_client.cpp              | 16 ++++++++++++++++
 rmw_fastrtps_cpp/src/rmw_service.cpp             | 16 ++++++++++++++++
 .../rmw_fastrtps_shared_cpp/rmw_common.hpp       | 12 ++++++++++++
 rmw_fastrtps_shared_cpp/src/rmw_client.cpp       | 14 ++++++++++++++
 rmw_fastrtps_shared_cpp/src/rmw_service.cpp      | 14 ++++++++++++++
 5 files changed, 72 insertions(+)

diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp
index e2f27a44d..aefb71f56 100644
--- a/rmw_fastrtps_cpp/src/rmw_client.cpp
+++ b/rmw_fastrtps_cpp/src/rmw_client.cpp
@@ -505,4 +505,20 @@ rmw_client_set_on_new_response_callback(
     callback,
     user_data);
 }
+
+rmw_ret_t
+rmw_client_get_actual_qos(
+  const rmw_client_t * client,
+  rmw_qos_profile_t * qos)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client,
+    client->implementation_identifier,
+    eprosima_fastrtps_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT);
+
+  return rmw_fastrtps_shared_cpp::__rmw_client_get_actual_qos(client, qos);
+}
 }  // extern "C"
diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp
index c7302161d..dc80712fb 100644
--- a/rmw_fastrtps_cpp/src/rmw_service.cpp
+++ b/rmw_fastrtps_cpp/src/rmw_service.cpp
@@ -504,4 +504,20 @@ rmw_service_set_on_new_request_callback(
     callback,
     user_data);
 }
+
+rmw_ret_t
+rmw_service_get_actual_qos(
+  const rmw_service_t * service,
+  rmw_qos_profile_t * qos)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service,
+    service->implementation_identifier,
+    eprosima_fastrtps_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT);
+
+  return rmw_fastrtps_shared_cpp::__rmw_service_get_actual_qos(service, qos);
+}
 }  // extern "C"
diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp
index d965f4f03..860ccd8da 100644
--- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp
+++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp
@@ -469,6 +469,12 @@ __rmw_service_set_on_new_request_callback(
   rmw_event_callback_t callback,
   const void * user_data);
 
+RMW_FASTRTPS_SHARED_CPP_PUBLIC
+rmw_ret_t
+__rmw_service_get_actual_qos(
+  const rmw_service_t * service,
+  rmw_qos_profile_t * qos);
+
 RMW_FASTRTPS_SHARED_CPP_PUBLIC
 rmw_ret_t
 __rmw_client_set_on_new_response_callback(
@@ -476,6 +482,12 @@ __rmw_client_set_on_new_response_callback(
   rmw_event_callback_t callback,
   const void * user_data);
 
+RMW_FASTRTPS_SHARED_CPP_PUBLIC
+rmw_ret_t
+__rmw_client_get_actual_qos(
+  const rmw_client_t * client,
+  rmw_qos_profile_t * qos);
+
 RMW_FASTRTPS_SHARED_CPP_PUBLIC
 rmw_ret_t
 __rmw_event_set_callback(
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp
index a0568f84c..d2135042c 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp
@@ -139,4 +139,18 @@ __rmw_client_set_on_new_response_callback(
     callback);
   return RMW_RET_OK;
 }
+
+rmw_ret_t
+__rmw_client_get_actual_qos(
+  const rmw_client_t * client,
+  rmw_qos_profile_t * qos)
+{
+  auto info = static_cast<CustomClientInfo *>(client->data);
+
+  eprosima::fastdds::dds::DataReader * fastdds_dr = info->response_reader_;
+
+  dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos);
+
+  return RMW_RET_OK;
+}
 }  // namespace rmw_fastrtps_shared_cpp
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp
index cfdb4dc76..35679cb46 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp
@@ -151,4 +151,18 @@ __rmw_service_set_on_new_request_callback(
     callback);
   return RMW_RET_OK;
 }
+
+rmw_ret_t
+__rmw_service_get_actual_qos(
+  const rmw_service_t * service,
+  rmw_qos_profile_t * qos)
+{
+  auto info = static_cast<CustomServiceInfo *>(service->data);
+
+  eprosima::fastdds::dds::DataReader * fastdds_dr = info->request_reader_;
+
+  dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos);
+
+  return RMW_RET_OK;
+}
 }  // namespace rmw_fastrtps_shared_cpp