From 3d4ee7e22149b2c11943c4a4516e33abd6ad1e2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ingo=20L=C3=BCtkebohle?= Date: Fri, 24 Apr 2020 06:31:07 +0200 Subject: [PATCH] Implement service info structure with timestamps. (#627) * Adapt to using rmw_service_info_t Signed-off-by: Luetkebohle Ingo (CR/AEX3) * Provide backwards compatibility. Signed-off-by: Luetkebohle Ingo (CR/AEX3) * Add service test Signed-off-by: Luetkebohle Ingo (CR/AEX3) * minimize whitespace Signed-off-by: Karsten Knese Co-authored-by: Karsten Knese Signed-off-by: Jorge Perez --- rcl/include/rcl/client.h | 9 +++++++++ rcl/include/rcl/service.h | 11 ++++++++++- rcl/src/rcl/client.c | 17 +++++++++++++++-- rcl/src/rcl/service.c | 17 +++++++++++++++-- rcl/test/CMakeLists.txt | 4 ++++ rcl/test/rcl/client_fixture.cpp | 4 ++-- rcl/test/rcl/test_service.cpp | 24 ++++++++++++++++++------ 7 files changed, 73 insertions(+), 13 deletions(-) diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index ebb96b499f..df5d72b6f9 100644 --- a/rcl/include/rcl/client.h +++ b/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, diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 4924eeee3c..6c0ecb8315 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -232,7 +232,7 @@ rcl_service_get_default_options(void); * [1] only if required when filling the request, avoided for fixed sizes * * \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, diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 889825cd8f..ea2d2dd183 100644 --- a/rcl/src/rcl/client.c +++ b/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) { diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 33f4788a4a..6dc1c79fcc 100644 --- a/rcl/src/rcl/service.c +++ b/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, diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 84507bc3f2..261f8bd829 100644 --- a/rcl/test/CMakeLists.txt +++ b/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() diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index a8aa9b7095..650b14bf45 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/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; diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index 694d370e74..b141ae6a82 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/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); }