From f2287ea61c42a587b90cb92030e81cbfa06ae2d2 Mon Sep 17 00:00:00 2001
From: Jose Tomas Lorente <jtlorente@ekumenlabs.com>
Date: Mon, 28 Sep 2020 19:37:35 -0300
Subject: [PATCH] Update service/client request/response API error returns
 (#450)

Signed-off-by: lobotuerk <jtlorente@ekumenlabs.com>
---
 rmw_fastrtps_shared_cpp/src/rmw_request.cpp  | 33 ++++++++++----------
 rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 33 ++++++++++----------
 2 files changed, 32 insertions(+), 34 deletions(-)

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp
index be28994f6..2c1ef8a64 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp
@@ -21,6 +21,7 @@
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "rmw/impl/cpp/macros.hpp"
 #include "rmw/types.h"
 
 #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp"
@@ -38,17 +39,16 @@ __rmw_send_request(
   const void * ros_request,
   int64_t * sequence_id)
 {
-  assert(client);
-  assert(ros_request);
-  assert(sequence_id);
+  RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client,
+    client->implementation_identifier, identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT);
 
   rmw_ret_t returnedValue = RMW_RET_ERROR;
 
-  if (client->implementation_identifier != identifier) {
-    RMW_SET_ERROR_MSG("node handle not from this implementation");
-    return RMW_RET_ERROR;
-  }
-
   auto info = static_cast<CustomClientInfo *>(client->data);
   assert(info);
 
@@ -77,18 +77,17 @@ __rmw_take_request(
   void * ros_request,
   bool * taken)
 {
-  assert(service);
-  assert(request_header);
-  assert(ros_request);
-  assert(taken);
+  RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service,
+    service->implementation_identifier, identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
 
   *taken = false;
 
-  if (service->implementation_identifier != identifier) {
-    RMW_SET_ERROR_MSG("service handle not from this implementation");
-    return RMW_RET_ERROR;
-  }
-
   auto info = static_cast<CustomServiceInfo *>(service->data);
   assert(info);
 
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp
index a91cf7276..4ee96af24 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp
@@ -20,6 +20,7 @@
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "rmw/impl/cpp/macros.hpp"
 
 #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp"
 #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp"
@@ -37,18 +38,17 @@ __rmw_take_response(
   void * ros_response,
   bool * taken)
 {
-  assert(client);
-  assert(request_header);
-  assert(ros_response);
-  assert(taken);
+  RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client,
+    client->implementation_identifier, identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
 
   *taken = false;
 
-  if (client->implementation_identifier != identifier) {
-    RMW_SET_ERROR_MSG("service handle not from this implementation");
-    return RMW_RET_ERROR;
-  }
-
   auto info = static_cast<CustomClientInfo *>(client->data);
   assert(info);
 
@@ -81,17 +81,16 @@ __rmw_send_response(
   rmw_request_id_t * request_header,
   void * ros_response)
 {
-  assert(service);
-  assert(request_header);
-  assert(ros_response);
+  RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service,
+    service->implementation_identifier, identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT);
 
   rmw_ret_t returnedValue = RMW_RET_ERROR;
 
-  if (service->implementation_identifier != identifier) {
-    RMW_SET_ERROR_MSG("service handle not from this implementation");
-    return RMW_RET_ERROR;
-  }
-
   auto info = static_cast<CustomServiceInfo *>(service->data);
   assert(info);