Client/Service: Check if reader/writer QoS match

Signed-off-by: Mauro Passerino <mpasserino@irobot.com>
ros2 · Oct 12, 2021 · 7004b81 · 7004b81
1 parent 48c7bad
commit 7004b81
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 4 deletions.
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp
@@ -135,8 +135,34 @@ __rmw_client_get_actual_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);
+  eprosima::fastdds::dds::DataWriter * fastdds_dw = info->request_writer_;
+
+  rmw_qos_profile_t dr_qos = rmw_qos_profile_default;
+  rmw_qos_profile_t dw_qos = rmw_qos_profile_default;
+
+  dds_qos_to_rmw_qos(fastdds_dr->get_qos(), &dr_qos);
+  dds_qos_to_rmw_qos(fastdds_dw->get_qos(), &dw_qos);
+
+  // Check if the QoS of the data reader and the data writer match
+  if (dr_qos.history == dw_qos.history &&
+    dr_qos.depth == dw_qos.depth &&
+    dr_qos.reliability == dw_qos.reliability &&
+    dr_qos.durability == dw_qos.durability &&
+    dr_qos.liveliness == dw_qos.liveliness &&
+    dr_qos.deadline.sec == dw_qos.deadline.sec &&
+    dr_qos.deadline.nsec == dw_qos.deadline.nsec &&
+    dr_qos.lifespan.sec == dw_qos.lifespan.sec &&
+    dr_qos.lifespan.nsec == dw_qos.lifespan.nsec &&
+    dr_qos.liveliness_lease_duration.sec == dw_qos.liveliness_lease_duration.sec &&
+    dr_qos.liveliness_lease_duration.nsec == dw_qos.liveliness_lease_duration.nsec)
+  {
+    // The client has a single QoS, we can set it either as the qos
+    // of the data reader or data writer, as they match.
+    dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos);
+  } else {
+    RMW_SET_ERROR_MSG("datawriter QoS does not match datareader QoS");
+    return RMW_RET_ERROR;
+  }
 
   return RMW_RET_OK;
 }

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp
@@ -147,8 +147,34 @@ __rmw_service_get_actual_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);
+  eprosima::fastdds::dds::DataWriter * fastdds_dw = info->response_writer_;
+
+  rmw_qos_profile_t dr_qos = rmw_qos_profile_default;
+  rmw_qos_profile_t dw_qos = rmw_qos_profile_default;
+
+  dds_qos_to_rmw_qos(fastdds_dr->get_qos(), &dr_qos);
+  dds_qos_to_rmw_qos(fastdds_dw->get_qos(), &dw_qos);
+
+  // Check if the QoS of the data reader and the data writer match
+  if (dr_qos.history == dw_qos.history &&
+    dr_qos.depth == dw_qos.depth &&
+    dr_qos.reliability == dw_qos.reliability &&
+    dr_qos.durability == dw_qos.durability &&
+    dr_qos.liveliness == dw_qos.liveliness &&
+    dr_qos.deadline.sec == dw_qos.deadline.sec &&
+    dr_qos.deadline.nsec == dw_qos.deadline.nsec &&
+    dr_qos.lifespan.sec == dw_qos.lifespan.sec &&
+    dr_qos.lifespan.nsec == dw_qos.lifespan.nsec &&
+    dr_qos.liveliness_lease_duration.sec == dw_qos.liveliness_lease_duration.sec &&
+    dr_qos.liveliness_lease_duration.nsec == dw_qos.liveliness_lease_duration.nsec)
+  {
+    // The service has a single QoS, we can set it either as the qos
+    // of the data reader or data writer, as they match.
+    dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos);
+  } else {
+    RMW_SET_ERROR_MSG("datawriter QoS does not match datareader QoS");
+    return RMW_RET_ERROR;
+  }
 
   return RMW_RET_OK;
 }