Implement service info structure with timestamps. (#627)

* Adapt to using rmw_service_info_t

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com>

* Provide backwards compatibility.

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com>

* Add service test

Signed-off-by: Luetkebohle Ingo (CR/AEX3) <ingo.luetkebohle@de.bosch.com>

* minimize whitespace

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

Co-authored-by: Karsten Knese <karsten@openrobotics.org>
Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>

rcl/include/rcl/client.h

@@ -285,6 +285,15 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
+rcl_take_response_with_info(
+  const rcl_client_t * client,
+  rmw_service_info_t * request_header,
+  void * ros_response);
+
+/// backwards compatibility function that takes a rmw_request_id_t only
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
 rcl_take_response(
   const rcl_client_t * client,
   rmw_request_id_t * request_header,

rcl/include/rcl/service.h

@@ -232,7 +232,7 @@ rcl_service_get_default_options(void);
  * <i>[1] only if required when filling the request, avoided for fixed sizes</i>
  *
  * \param[in] service the handle to the service from which to take
- * \param[inout] request_header ptr to the struct holding metadata about the request ID
+ * \param[inout] request_header ptr to the struct holding metadata about the request
  * \param[inout] ros_request type-erased ptr to an allocated ROS request message
  * \return `RCL_RET_OK` if the request was taken, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@@ -245,6 +245,15 @@ rcl_service_get_default_options(void);
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
+rcl_take_request_with_info(
+  const rcl_service_t * service,
+  rmw_service_info_t * request_header,
+  void * ros_request);
+
+/// backwards compatibility version that takes a request_id only
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
 rcl_take_request(
   const rcl_service_t * service,
   rmw_request_id_t * request_header,

rcl/src/rcl/client.c

@@ -289,9 +289,9 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t
 }
 
 rcl_ret_t
-rcl_take_response(
+rcl_take_response_with_info(
   const rcl_client_t * client,
-  rmw_request_id_t * request_header,
+  rmw_service_info_t * request_header,
   void * ros_response)
 {
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response");
@@ -317,6 +317,19 @@ rcl_take_response(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_take_response(
+  const rcl_client_t * client,
+  rmw_request_id_t * request_header,
+  void * ros_response)
+{
+  rmw_service_info_t header;
+  header.request_id = *request_header;
+  rcl_ret_t ret = rcl_take_response_with_info(client, &header, ros_response);
+  *request_header = header.request_id;
+  return ret;
+}
+
 bool
 rcl_client_is_valid(const rcl_client_t * client)
 {

rcl/src/rcl/service.c

@@ -276,9 +276,9 @@ rcl_service_get_rmw_handle(const rcl_service_t * service)
 }
 
 rcl_ret_t
-rcl_take_request(
+rcl_take_request_with_info(
   const rcl_service_t * service,
-  rmw_request_id_t * request_header,
+  rmw_service_info_t * request_header,
   void * ros_request)
 {
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service server taking service request");
@@ -308,6 +308,19 @@ rcl_take_request(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_take_request(
+  const rcl_service_t * service,
+  rmw_request_id_t * request_header,
+  void * ros_request)
+{
+  rmw_service_info_t header;
+  header.request_id = *request_header;
+  rcl_ret_t ret = rcl_take_request_with_info(service, &header, ros_request);
+  *request_header = header.request_id;
+  return ret;
+}
+
 rcl_ret_t
 rcl_send_response(
   const rcl_service_t * service,

rcl/test/CMakeLists.txt

@@ -210,11 +210,15 @@ function(test_target_function)
     message(STATUS "Enabling message timestamp test for ${rmw_implementation}")
     target_compile_definitions(test_subscription${target_suffix}
       PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1" "RMW_RECEIVED_TIMESTAMP_SUPPORTED=1")
+    target_compile_definitions(test_service${target_suffix}
+      PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1" "RMW_RECEIVED_TIMESTAMP_SUPPORTED=1")
   else()
     if(rmw_implementation STREQUAL "rmw_cyclonedds_cpp")
       message(STATUS "Enabling only source timestamp test for ${rmw_implementation}")
       target_compile_definitions(test_subscription${target_suffix}
         PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1")
+      target_compile_definitions(test_service${target_suffix}
+        PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1")
     else()
       message(STATUS "Disabling message timestamp test for ${rmw_implementation}")
     endif()

rcl/test/rcl/client_fixture.cpp

@@ -218,8 +218,8 @@ int main(int argc, char ** argv)
       RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Client never became ready");
       return -1;
     }
-    rmw_request_id_t header;
-    if (rcl_take_response(&client, &header, &client_response) != RCL_RET_OK) {
+    rmw_service_info_t header;
+    if (rcl_take_response_with_info(&client, &header, &client_response) != RCL_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
         ROS_PACKAGE_NAME, "Error in send response: %s", rcl_get_error_string().str);
       return -1;

rcl/test/rcl/test_service.cpp

@@ -205,15 +205,15 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
     // Initialize a separate instance of the request and take the pending request.
     test_msgs__srv__BasicTypes_Request service_request;
     test_msgs__srv__BasicTypes_Request__init(&service_request);
-    rmw_request_id_t header;
-    ret = rcl_take_request(&service, &header, &service_request);
+    rmw_service_info_t header;
+    ret = rcl_take_request_with_info(&service, &header, &service_request);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
     EXPECT_EQ(1, service_request.uint8_value);
     EXPECT_EQ(2UL, service_request.uint32_value);
     // Simulate a response callback by summing the request and send the response..
     service_response.uint64_value = service_request.uint8_value + service_request.uint32_value;
-    ret = rcl_send_response(&service, &header, &service_response);
+    ret = rcl_send_response(&service, &header.request_id, &service_response);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
     test_msgs__srv__BasicTypes_Request__fini(&service_request);
   }
@@ -223,10 +223,22 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
   test_msgs__srv__BasicTypes_Response client_response;
   test_msgs__srv__BasicTypes_Response__init(&client_response);
 
-  rmw_request_id_t header;
-  ret = rcl_take_response(&client, &header, &client_response);
+  rmw_service_info_t header;
+  ret = rcl_take_response_with_info(&client, &header, &client_response);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   EXPECT_EQ(client_response.uint64_value, 3ULL);
-  EXPECT_EQ(header.sequence_number, 1);
+  EXPECT_EQ(header.request_id.sequence_number, 1);
+#ifdef RMW_TIMESTAMPS_SUPPORTED
+  EXPECT_NE(0u, header.source_timestamp);
+#ifdef RMW_RECEIVED_TIMESTAMP_SUPPORTED
+  EXPECT_NE(0u, header.received_timestamp);
+#else
+  EXPECT_EQ(0u, header.received_timestamp);
+#endif
+#else
+  EXPECT_EQ(0u, header.source_timestamp);
+  EXPECT_EQ(0u, header.received_timestamp);
+#endif
+
   test_msgs__srv__BasicTypes_Response__fini(&client_response);
 }