Add apis to retrieve service/client QoS

rcl/include/rcl/client.h

@@ -441,6 +441,32 @@ rcl_client_set_on_new_response_callback(
   rcl_event_callback_t callback,
   const void * user_data);
 
+/// Get the actual qos settings of the client.
+/**
+ * Used to get the actual qos settings of the client.
+ * The actual configuration applied when using RMW_*_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_*_UNKNOWN.
+ * The returned struct is only valid as long as the rcl_client_t is valid.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] client pointer to the rcl client
+ * \return qos struct if successful, otherwise `NULL`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const rmw_qos_profile_t *
+rcl_client_get_actual_qos(const rcl_client_t * client);
+
 #ifdef __cplusplus
 }
 #endif

rcl/include/rcl/service.h

@@ -472,6 +472,32 @@ rcl_service_set_on_new_request_callback(
   rcl_event_callback_t callback,
   const void * user_data);
 
+/// Get the actual qos settings of the service.
+/**
+ * Used to get the actual qos settings of the service.
+ * The actual configuration applied when using RMW_*_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_*_UNKNOWN.
+ * The returned struct is only valid as long as the rcl_service_t is valid.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] service pointer to the rcl service
+ * \return qos struct if successful, otherwise `NULL`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const rmw_qos_profile_t *
+rcl_service_get_actual_qos(const rcl_service_t * service);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/client.c

@@ -36,6 +36,7 @@ extern "C"
 typedef struct rcl_client_impl_t
 {
   rcl_client_options_t options;
+  rmw_qos_profile_t actual_qos;
   rmw_client_t * rmw_handle;
   atomic_int_least64_t sequence_number;
 } rcl_client_impl_t;
@@ -99,6 +100,19 @@ rcl_client_init(
     sizeof(rcl_client_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
     client->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
+  // Check the qos profile. If some fields are set as system default,
+  // it can happen that the DDS chooses different QoS policies for entities
+  // belonging to the client. The QoS of the client should match the QoS of
+  // all entities belonging to it.
+  const rmw_qos_profile_t * qos = &options->qos;
+  if (qos->history == RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT ||
+    qos->reliability == RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT ||
+    qos->durability == RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT ||
+    qos->depth == RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT)
+  {
+    RCL_SET_ERROR_MSG("system default qos not supported on clients");
+    goto fail;
+  }
   // Fill out implementation struct.
   // rmw handle (create rmw client)
   // TODO(wjwwood): pass along the allocator to rmw when it supports it
@@ -111,6 +125,23 @@ rcl_client_init(
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     goto fail;
   }
+  // get actual qos, and store it
+  rmw_ret_t rmw_ret = rmw_client_get_actual_qos(
+    client->impl->rmw_handle,
+    &client->impl->actual_qos);
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    goto fail;
+  }
+  // ROS specific namespacing conventions avoidance
+  // is not retrieved by get_actual_qos
+  client->impl->actual_qos.avoid_ros_namespace_conventions =
+    options->qos.avoid_ros_namespace_conventions;
+
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    goto fail;
+  }
   // options
   client->impl->options = *options;
   atomic_init(&client->impl->sequence_number, 0);
@@ -298,6 +329,15 @@ rcl_client_set_on_new_response_callback(
     user_data);
 }
 
+const rmw_qos_profile_t *
+rcl_client_get_actual_qos(const rcl_client_t * client)
+{
+  if (!rcl_client_is_valid(client)) {
+    return NULL;
+  }
+  return &client->impl->actual_qos;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/service.c

@@ -33,6 +33,7 @@ extern "C"
 typedef struct rcl_service_impl_t
 {
   rcl_service_options_t options;
+  rmw_qos_profile_t actual_qos;
   rmw_service_t * rmw_handle;
 } rcl_service_impl_t;
 
@@ -110,6 +111,19 @@ rcl_service_init(
       "Warning: Setting QoS durability to 'transient local' for service servers "
       "can cause them to receive requests from clients that have since terminated.");
   }
+  // Check the qos profile. If some fields are set as system default,
+  // it can happen that the DDS chooses different QoS policies for entities
+  // belonging to the service. The QoS of the service should match the QoS of
+  // all entities belonging to it.
+  const rmw_qos_profile_t * qos = &options->qos;
+  if (qos->history == RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT ||
+    qos->reliability == RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT ||
+    qos->durability == RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT ||
+    qos->depth == RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT)
+  {
+    RCL_SET_ERROR_MSG("system default qos not supported on clients");
+    goto fail;
+  }
   // Fill out implementation struct.
   // rmw handle (create rmw service)
   // TODO(wjwwood): pass along the allocator to rmw when it supports it
@@ -122,6 +136,17 @@ rcl_service_init(
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     goto fail;
   }
+  // get actual qos, and store it
+  rmw_ret_t rmw_ret = rmw_service_get_actual_qos(
+    service->impl->rmw_handle,
+    &service->impl->actual_qos);
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    goto fail;
+  }
+  // ROS specific namespacing conventions is not retrieved by get_actual_qos
+  service->impl->actual_qos.avoid_ros_namespace_conventions =
+    options->qos.avoid_ros_namespace_conventions;
   // options
   service->impl->options = *options;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized");
@@ -318,6 +343,15 @@ rcl_service_set_on_new_request_callback(
     user_data);
 }
 
+const rmw_qos_profile_t *
+rcl_service_get_actual_qos(const rcl_service_t * service)
+{
+  if (!rcl_service_is_valid(service)) {
+    return NULL;
+  }
+  return &service->impl->actual_qos;
+}
+
 
 #ifdef __cplusplus
 }