From cc8cb4ffaaa992a6575333e806c6da612d192f06 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 23 Jun 2022 10:11:06 -0700 Subject: [PATCH 01/16] introspection prototyping Signed-off-by: Brian Chen --- rcl/include/rcl/introspection.h | 25 ++++++ rcl/src/rcl/introspection.c | 73 ++++++++++++++++ rcl/src/rcl/service.c | 145 ++++++++++++++++++++++++++++++++ 3 files changed, 243 insertions(+) create mode 100644 rcl/include/rcl/introspection.h create mode 100644 rcl/src/rcl/introspection.c diff --git a/rcl/include/rcl/introspection.h b/rcl/include/rcl/introspection.h new file mode 100644 index 000000000..7cb010e22 --- /dev/null +++ b/rcl/include/rcl/introspection.h @@ -0,0 +1,25 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/service.h" +#include "rcl/macros.h" + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +send_introspection_message( + rcl_service_t * service, + void * service_payload, + rmw_request_id_t * header, + rcl_allocator_t * allocator); diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c new file mode 100644 index 000000000..776327dc7 --- /dev/null +++ b/rcl/src/rcl/introspection.c @@ -0,0 +1,73 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/service.h" +#include "rcl/introspection.h" +#include "rcl_interfaces/msg/service_event.h" +#include "rcl/macros.h" +#include "rmw/rmw.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rosidl_runtime_c/string_functions.h" +#include "publisher.h" + +rcl_ret_t +send_introspection_message( + rcl_service_t * service, + void * service_payload, + rmw_request_id_t * header, + rcl_allocator_t * allocator) +{ + rcl_ret_t ret; + rcl_serialized_message_t serialized_message; + ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); // why is this 0u? + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + rcl_interfaces__msg__ServiceEvent msg; + rcl_interfaces__msg__ServiceEvent__init(msg); + msg.event_type; + msg.client_id = header->request_id; + msg.sequence_number = header->sequence_numberl + msg.stamp; + + + rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); + rosidl_runtime_c__String__assign(&msg.event_name, rcl_service_get); // ..._{Request, Response}); // TODO(ihasdapie): impl this + rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); + memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); + + ret = rcl_publish(service->impl->service_event_publisher, &serialized_message, NULL); + if (RMW_RET_OK != ret) { // there has to be a macro for this right? + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + ret = rmw_serialized_message_fini(&serialized_message); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + // have to fini the request/response message as well + rcl_interfaces__msg__ServiceEvent__fini(&msg); + + + + + return ret; +} + + diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index e1bdb0174..0eaca3b83 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rcl/time.h" #ifdef __cplusplus extern "C" { @@ -19,16 +20,24 @@ extern "C" #include "rcl/service.h" +#include "rcl/types.h" #include #include #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/publisher.h" +#include #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "tracetools/tracetools.h" +#include "rcl_interfaces/msg/service_event.h" +// #include "rcl_interfaces/msg/service_event.h" // why does it complain here?? +#include "builtin_interfaces/msg/time.h" +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" struct rcl_service_impl_s { @@ -36,6 +45,8 @@ struct rcl_service_impl_s rmw_qos_profile_t actual_request_subscription_qos; rmw_qos_profile_t actual_response_publisher_qos; rmw_service_t * rmw_handle; + rcl_publisher_t * service_event_publisher; + rcl_clock_t * clock; }; rcl_service_t @@ -124,6 +135,46 @@ rcl_service_init( RCL_SET_ERROR_MSG(rmw_get_error_string().str); goto fail; } + + + // build service event publisher + char* service_event_name = allocator->allocate(sizeof(char) * (strlen(remapped_service_name) + + strlen("/_service_event") + 1), allocator->state); + strcpy(service_event_name, remapped_service_name); + strcat(service_event_name, "/_service_event"); + *service->impl->service_event_publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * service_event_typesupport = + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(service->impl->service_event_publisher, node, + service_event_typesupport, service_event_name, &publisher_options); + + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + goto fail; + } + + + // make a clock + ret = rcl_clock_init(RCL_STEADY_TIME, service->impl->clock, &allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + goto fail; + } + + + // what needs to be done: + // In rcl_service_init: + // - get the rcl_message_type_support_t from the rcl_service_type_support_t + // - create rcl_publishers_options from given things + // - decide on how to overhaul rosidl to support thisj + // Hook into send/take + // - build a new message on request send/recieve/etc + // - pub it out + // Other: + // Parameters + + // get actual qos, and store it rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos( service->impl->rmw_handle, @@ -169,6 +220,7 @@ rcl_service_init( // Fall through to clean up cleanup: allocator->deallocate(remapped_service_name, allocator->state); + allocator->deallocate(service_event_name, allocator->state); return ret; } @@ -201,6 +253,10 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) allocator.deallocate(service->impl, allocator.state); service->impl = NULL; } + + rcl_publisher_fini(service->impl->service_event_publisher, node); + rcl_clock_fini(service->impl->clock); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized"); return result; } @@ -293,6 +349,78 @@ rcl_take_request( return ret; } + +rcl_ret_t +send_introspection_message( + rcl_service_t * service, + void * ros_response_request, + rmw_request_id_t * header, + rcl_allocator_t * allocator) +{ + // need a copy of this function for request/response (?) and also in client.c unless there's + // a utils.c or something... + + rcl_ret_t ret; + rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); + ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); // why is this 0u? + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + // how to get type from service_payload? + rosidl_message_type_support_t * type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(PkgName, MsgSubfolder, MsgName); + + ret = rmw_serialize(ros_response_request, typesupport, &serialized_message) + + + + rcl_time_point_value_t now; + ret = rcl_clock_get_now(service->impl->clock, &now) + if (RMW_RET_OK != ret) { // there has to be a macro for this right? + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + rcl_interfaces__msg__ServiceEvent msg; + rcl_interfaces__msg__ServiceEvent__init(msg); + + + + + + builtin_interfaces__msg__Time timestamp; + builtin_interfaces__msg__Time__init(×tamp); + timestamp.nanosec = now; + // is there a rcl method for getting the seconds too? + msg.event_type; + msg.client_id = header->writer_guid; + msg.sequence_number = header->sequence_number; + msg.stamp = timestamp; + + rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); + rosidl_runtime_c__String__assign(&msg.event_name, rcl_service_get); // ..._{Request, Response}); // TODO(ihasdapie): impl this + rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); + memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); + + ret = rcl_publish(service->impl->service_event_publisher, &serialized_message, NULL); + if (RMW_RET_OK != ret) { // there has to be a macro for this right? + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + ret = rmw_serialized_message_fini(&serialized_message); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + // have to fini the request/response message here as well + rcl_interfaces__msg__ServiceEvent__fini(&msg); + + return ret; +} + rcl_ret_t rcl_send_response( const rcl_service_t * service, @@ -308,12 +436,29 @@ rcl_send_response( const rcl_service_options_t * options = rcl_service_get_options(service); RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); + // publish out the introspected content + send_introspection_message( + service, + ros_response, + request_header, + &options->allocator); + + + + if (rmw_send_response( service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + + // need a message form of ros_response as well. We have a response here but we want to be able to publish + // out a message as well. For now let's hardcode one in? + { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } return RCL_RET_OK; } From 16607a6526d6d30c29c2c440b65f3753992665e9 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 23 Jun 2022 16:44:14 -0700 Subject: [PATCH 02/16] more prototyping Signed-off-by: Brian Chen --- rcl/src/rcl/service.c | 35 ++++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 0eaca3b83..f9a40d7c4 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -32,16 +32,16 @@ extern "C" #include "rcutils/macros.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rcl/graph.h" #include "tracetools/tracetools.h" -#include "rcl_interfaces/msg/service_event.h" -// #include "rcl_interfaces/msg/service_event.h" // why does it complain here?? +#include "rcl_interfaces/msg/service_event_type.h" #include "builtin_interfaces/msg/time.h" #include "rosidl_runtime_c/string_functions.h" #include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rcl_interfaces/msg/service_event.h" struct rcl_service_impl_s { - rcl_service_options_t options; rmw_qos_profile_t actual_request_subscription_qos; rmw_qos_profile_t actual_response_publisher_qos; rmw_service_t * rmw_handle; @@ -354,6 +354,7 @@ rcl_ret_t send_introspection_message( rcl_service_t * service, void * ros_response_request, + uint8_t event_type, rmw_request_id_t * header, rcl_allocator_t * allocator) { @@ -368,15 +369,16 @@ send_introspection_message( return RCL_RET_ERROR; } - // how to get type from service_payload? - rosidl_message_type_support_t * type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(PkgName, MsgSubfolder, MsgName); + // TODO: how to get the message_type_support for a request/response? + // We could get the service_type_support but how to build a message_type_support from that? + rosidl_message_type_support_t * type_support = NULL; - ret = rmw_serialize(ros_response_request, typesupport, &serialized_message) + ret = rmw_serialize(ros_response_request, type_support, &serialized_message) rcl_time_point_value_t now; - ret = rcl_clock_get_now(service->impl->clock, &now) + ret = rcl_clock_get_now(service->impl->clock, &now); if (RMW_RET_OK != ret) { // there has to be a macro for this right? RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -384,22 +386,22 @@ send_introspection_message( rcl_interfaces__msg__ServiceEvent msg; rcl_interfaces__msg__ServiceEvent__init(msg); - - - - builtin_interfaces__msg__Time timestamp; builtin_interfaces__msg__Time__init(×tamp); timestamp.nanosec = now; // is there a rcl method for getting the seconds too? - msg.event_type; + msg.event_type = event_type; msg.client_id = header->writer_guid; msg.sequence_number = header->sequence_number; msg.stamp = timestamp; rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); - rosidl_runtime_c__String__assign(&msg.event_name, rcl_service_get); // ..._{Request, Response}); // TODO(ihasdapie): impl this + + + + // ..._{Request, Response}); // TODO(ihasdapie): figure out how to get the message name. Type is in event_type + rosidl_runtime_c__String__assign(&msg.event_name, "ServiceEvent__{Request, Response}"); rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); @@ -415,6 +417,12 @@ send_introspection_message( return RCL_RET_ERROR; } + ret = rmw_serialized_message_fini(&serialized_message); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + // have to fini the request/response message here as well rcl_interfaces__msg__ServiceEvent__fini(&msg); @@ -439,6 +447,7 @@ rcl_send_response( // publish out the introspected content send_introspection_message( service, + rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT; ros_response, request_header, &options->allocator); From 1076d8c3629302f7715c27a6a74ee8061af935f4 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 24 Jun 2022 23:23:48 -0700 Subject: [PATCH 03/16] introspection message published but segfaulting after first one published Signed-off-by: Brian Chen --- rcl/CMakeLists.txt | 1 + rcl/src/rcl/introspection.c | 129 ++++++++++++++++----- rcl/src/rcl/introspection.h | 45 ++++++++ rcl/src/rcl/service.c | 222 ++++++++++++++---------------------- rcl/src/rcl/service_impl.h | 32 ++++++ 5 files changed, 264 insertions(+), 165 deletions(-) create mode 100644 rcl/src/rcl/introspection.h create mode 100644 rcl/src/rcl/service_impl.h diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 11462f119..b4891fa13 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -64,6 +64,7 @@ set(${PROJECT_NAME}_sources src/rcl/validate_enclave_name.c src/rcl/validate_topic_name.c src/rcl/wait.c + src/rcl/introspection.c ) add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}) diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 776327dc7..6394e944c 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -12,62 +12,133 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rcl/service.h" -#include "rcl/introspection.h" -#include "rcl_interfaces/msg/service_event.h" -#include "rcl/macros.h" -#include "rmw/rmw.h" -#include "rosidl_runtime_c/primitives_sequence_functions.h" + +#ifndef RCL_INTROSPECTION_H_ +#define RCL_INTROSPECTION_H_ + + +#include "./service_impl.h" + +#include "rcl/error_handling.h" +#include "rmw/error_handling.h" + +#include "rcutils/macros.h" +#include "rcutils/logging_macros.h" #include "rosidl_runtime_c/string_functions.h" -#include "publisher.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + +#include "unique_identifier_msgs/msg/uuid.h" +#include "builtin_interfaces/msg/time.h" +#include "rcl_interfaces/msg/service_event.h" +#include "rcl_interfaces/msg/service_event_type.h" + + rcl_ret_t send_introspection_message( - rcl_service_t * service, - void * service_payload, + const rcl_service_t * service, + uint8_t event_type, + void * ros_response_request, rmw_request_id_t * header, - rcl_allocator_t * allocator) + const rcl_service_options_t * options) { + // need a copy of this function for request/response (?) and also in client.c unless there's + // a utils.c or something... + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Sending introspection message -----------------"); + rcl_ret_t ret; - rcl_serialized_message_t serialized_message; - ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); // why is this 0u? + rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); + + ret = rmw_serialized_message_init(&serialized_message, 0u, &options->allocator); // why is this 0u? if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message -----------------"); rcl_interfaces__msg__ServiceEvent msg; - rcl_interfaces__msg__ServiceEvent__init(msg); - msg.event_type; - msg.client_id = header->request_id; - msg.sequence_number = header->sequence_numberl - msg.stamp; + rcl_interfaces__msg__ServiceEvent__init(&msg); + msg.event_type = event_type; + msg.sequence_number = header->sequence_number; + rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); + printf("sequence_number: %ld\n", msg.sequence_number); + printf("service_name: %s\n", msg.service_name.data); + printf("event_type: %d\n", msg.event_type); + + rosidl_runtime_c__String__assign(&msg.event_name, "AddTwoInts"); // TODO(ihasdapie): get this properly, the type name is hardcoded throughout right now + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building time message -----------------"); + builtin_interfaces__msg__Time timestamp; + builtin_interfaces__msg__Time__init(×tamp); - rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); - rosidl_runtime_c__String__assign(&msg.event_name, rcl_service_get); // ..._{Request, Response}); // TODO(ihasdapie): impl this + rcl_time_point_value_t now; + ret = rcl_clock_get_now(service->impl->introspection_utils->clock, &now); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + // is there a rcl method for getting the seconds too? + timestamp.sec = RCL_NS_TO_S(now); + timestamp.nanosec = now % (1000LL * 1000LL * 1000LL); + msg.stamp = timestamp; + + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building UUID message -----------------"); + unique_identifier_msgs__msg__UUID uuid; + unique_identifier_msgs__msg__UUID__init(&uuid); + memcpy(uuid.uuid, header->writer_guid, 16 * sizeof(uint8_t)); + msg.client_id = uuid; + + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Serializing introspected message -----------------"); + + rosidl_message_type_support_t * serialized_message_ts; + switch (event_type) { + case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: + serialized_message_ts = service->impl->introspection_utils->request_type_support; + break; + case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: + serialized_message_ts = service->impl->introspection_utils->response_type_support; + break; + case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED: + serialized_message_ts = service->impl->introspection_utils->response_type_support; + break; + case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: + serialized_message_ts = service->impl->introspection_utils->request_type_support; + break; + default: + RCL_SET_ERROR_MSG("Invalid event type"); + return RCL_RET_ERROR; + } + + ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: memcpying introspected message -----------------"); + printf("serialized_message.buffer_length: %zu, serialized_message size: %zu\n", serialized_message.buffer_length, serialized_message.buffer_capacity); rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); + + // This segfaults, but thought this should be ok? since serialized_message.buffer is a uint8_t* which _should_ be the same as a byte array + // rosidl_runtime_c__octet__Sequence__copy((rosidl_runtime_c__octet__Sequence *) serialized_message.buffer, &msg.serialized_event); + memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); - ret = rcl_publish(service->impl->service_event_publisher, &serialized_message, NULL); - if (RMW_RET_OK != ret) { // there has to be a macro for this right? + + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Publishing ServiceEvent Message -----------------"); + ret = rcl_publish(service->impl->introspection_utils->publisher, &msg, NULL); + if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Cleanup -----------------"); + + unique_identifier_msgs__msg__UUID__fini(&uuid); + builtin_interfaces__msg__Time__fini(×tamp); ret = rmw_serialized_message_fini(&serialized_message); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - - // have to fini the request/response message as well + // have to fini the request/response message here as well rcl_interfaces__msg__ServiceEvent__fini(&msg); - - - - - return ret; + return RCL_RET_OK; } +#endif // RCL_INTROSPECTION_H_ diff --git a/rcl/src/rcl/introspection.h b/rcl/src/rcl/introspection.h new file mode 100644 index 000000000..f3f1c5f84 --- /dev/null +++ b/rcl/src/rcl/introspection.h @@ -0,0 +1,45 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef RCL__SERVICE_INTROSPECTION_H_ +#define RCL__SERVICE_INTROSPECTION_H_ + +#include "rcl/time.h" +#include "rcl/publisher.h" +#include "rcl/service.h" +#include "rmw/rmw.h" + + + +#define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" + +typedef struct rcl_service_introspection_utils_s { + rcl_clock_t * clock; + rcl_publisher_t * publisher; + rosidl_message_type_support_t * request_type_support; + rosidl_message_type_support_t * response_type_support; +} rcl_service_introspection_utils_t; + + +rcl_ret_t +send_introspection_message( + const rcl_service_t * service, + uint8_t event_type, + void * ros_response_request, + rmw_request_id_t * header, + const rcl_service_options_t * options); + + +#endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index f9a40d7c4..221ba5011 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -12,42 +12,43 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rcl/time.h" #ifdef __cplusplus extern "C" { #endif -#include "rcl/service.h" - -#include "rcl/types.h" +#include #include #include +#include "rcl/publisher.h" + +#include "rcl/time.h" +#include "rcl/types.h" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/graph.h" #include "rcl/publisher.h" -#include -#include "rcutils/logging_macros.h" -#include "rcutils/macros.h" + #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rcl/graph.h" + +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" + #include "tracetools/tracetools.h" + + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_runtime_c/message_type_support_struct.h" + #include "rcl_interfaces/msg/service_event_type.h" -#include "builtin_interfaces/msg/time.h" -#include "rosidl_runtime_c/string_functions.h" -#include "rosidl_runtime_c/primitives_sequence_functions.h" #include "rcl_interfaces/msg/service_event.h" -struct rcl_service_impl_s -{ - rmw_qos_profile_t actual_request_subscription_qos; - rmw_qos_profile_t actual_response_publisher_qos; - rmw_service_t * rmw_handle; - rcl_publisher_t * service_event_publisher; - rcl_clock_t * clock; -}; + + +#include "./service_impl.h" + rcl_service_t rcl_get_zero_initialized_service() @@ -93,6 +94,9 @@ rcl_service_init( // Expand and remap the given service name. char * remapped_service_name = NULL; + char * service_event_name = NULL; + // Preallocate space for for service_event_name + rcl_ret_t ret = rcl_node_resolve_name( node, service_name, @@ -108,12 +112,14 @@ rcl_service_init( } goto cleanup; } + RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); // Allocate space for the implementation struct. service->impl = (rcl_service_impl_t *)allocator->allocate( sizeof(rcl_service_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( service->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); @@ -123,6 +129,8 @@ 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."); } + + // Fill out implementation struct. // rmw handle (create rmw service) // TODO(wjwwood): pass along the allocator to rmw when it supports it @@ -136,44 +144,58 @@ rcl_service_init( goto fail; } - - // build service event publisher - char* service_event_name = allocator->allocate(sizeof(char) * (strlen(remapped_service_name) - + strlen("/_service_event") + 1), allocator->state); + // TODO(ihasdapie): Wrap this up as a fn introspection.c w/ `initialize_introspection_utils` and _fini, etc + service->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( + sizeof(rcl_service_introspection_utils_t), allocator->state); + // rcl_service_introspection_utils_t * iutils = service->impl->introspection_utils; + + // the reason why we can't use a macro and concatenate the Type to get what we want is + // because there we had always {goal status result} but here we can have any service type name... + // TODO(ihasdapie): Need to get the package and message name somehow + char * tsfile = "libexample_interfaces__rosidl_typesupport_c.so"; + void* tslib = dlopen(tsfile, RTLD_LAZY); + RCL_CHECK_FOR_NULL_WITH_MSG(tslib, "dlopen failed", ret = RCL_RET_ERROR; goto cleanup); + // there are macros for building these in rosidl_typesupport_interface but still need the msg package name + const char* ts_req_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Request"; + const char* ts_resp_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Response"; + + // TODO(ihasdapie): squash warning "ISO C forbids conversion of function pointer to object pointer type" + rosidl_message_type_support_t* (* ts_func_handle)() = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_resp_func_name); + RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", ret = RCL_RET_ERROR; goto cleanup); + service->impl->introspection_utils->response_type_support = ts_func_handle(); + ts_func_handle = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_req_func_name); + RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", ret = RCL_RET_ERROR; goto cleanup); + service->impl->introspection_utils->request_type_support = ts_func_handle(); + + service_event_name = (char*) allocator->zero_allocate( + strlen(remapped_service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, + sizeof(char), allocator->state); strcpy(service_event_name, remapped_service_name); - strcat(service_event_name, "/_service_event"); - *service->impl->service_event_publisher = rcl_get_zero_initialized_publisher(); - const rosidl_message_type_support_t * service_event_typesupport = - ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); + strcat(service_event_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + + // Make a publisher + service->impl->introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + *service->impl->introspection_utils->publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * service_event_typesupport = + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); // ROSIDL_GET_MSG_TYPE_SUPPORT gives an int?? And complier warns that it is being converted to a ptr rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(service->impl->service_event_publisher, node, + ret = rcl_publisher_init(service->impl->introspection_utils->publisher, node, service_event_typesupport, service_event_name, &publisher_options); + // there has to be a macro for this, right? if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); goto fail; } - // make a clock - ret = rcl_clock_init(RCL_STEADY_TIME, service->impl->clock, &allocator); + service->impl->introspection_utils->clock = allocator->allocate(sizeof(rcl_clock_t), allocator->state); + ret = rcl_clock_init(RCL_STEADY_TIME, service->impl->introspection_utils->clock, allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); goto fail; } - - - // what needs to be done: - // In rcl_service_init: - // - get the rcl_message_type_support_t from the rcl_service_type_support_t - // - create rcl_publishers_options from given things - // - decide on how to overhaul rosidl to support thisj - // Hook into send/take - // - build a new message on request send/recieve/etc - // - pub it out - // Other: - // Parameters - + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Clock initialized"); // get actual qos, and store it rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos( @@ -213,8 +235,11 @@ rcl_service_init( goto cleanup; fail: if (service->impl) { + allocator->deallocate(service->impl->introspection_utils, allocator->state); allocator->deallocate(service->impl, allocator->state); + service->impl->introspection_utils = NULL; service->impl = NULL; + // iutils= NULL; } ret = fail_ret; // Fall through to clean up @@ -250,12 +275,19 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + allocator.deallocate(service->impl->introspection_utils, allocator.state); allocator.deallocate(service->impl, allocator.state); + service->impl->introspection_utils = NULL; service->impl = NULL; + + ret = rcl_publisher_fini(service->impl->introspection_utils->publisher, node); + ret = rcl_clock_fini(service->impl->introspection_utils->clock); + if (ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } } - rcl_publisher_fini(service->impl->service_event_publisher, node); - rcl_clock_fini(service->impl->clock); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized"); return result; @@ -318,6 +350,13 @@ rcl_take_request_with_info( const rcl_service_options_t * options = rcl_service_get_options(service); RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); + /* send_introspection_message( + service, + rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED, + ros_request, + request_header, + options); */ + bool taken = false; rmw_ret_t ret = rmw_take_request( service->impl->rmw_handle, request_header, ros_request, &taken); @@ -350,85 +389,6 @@ rcl_take_request( } -rcl_ret_t -send_introspection_message( - rcl_service_t * service, - void * ros_response_request, - uint8_t event_type, - rmw_request_id_t * header, - rcl_allocator_t * allocator) -{ - // need a copy of this function for request/response (?) and also in client.c unless there's - // a utils.c or something... - - rcl_ret_t ret; - rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); - ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); // why is this 0u? - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - - // TODO: how to get the message_type_support for a request/response? - // We could get the service_type_support but how to build a message_type_support from that? - rosidl_message_type_support_t * type_support = NULL; - - ret = rmw_serialize(ros_response_request, type_support, &serialized_message) - - - - rcl_time_point_value_t now; - ret = rcl_clock_get_now(service->impl->clock, &now); - if (RMW_RET_OK != ret) { // there has to be a macro for this right? - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - - rcl_interfaces__msg__ServiceEvent msg; - rcl_interfaces__msg__ServiceEvent__init(msg); - - builtin_interfaces__msg__Time timestamp; - builtin_interfaces__msg__Time__init(×tamp); - timestamp.nanosec = now; - // is there a rcl method for getting the seconds too? - msg.event_type = event_type; - msg.client_id = header->writer_guid; - msg.sequence_number = header->sequence_number; - msg.stamp = timestamp; - - rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); - - - - // ..._{Request, Response}); // TODO(ihasdapie): figure out how to get the message name. Type is in event_type - rosidl_runtime_c__String__assign(&msg.event_name, "ServiceEvent__{Request, Response}"); - rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); - memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); - - ret = rcl_publish(service->impl->service_event_publisher, &serialized_message, NULL); - if (RMW_RET_OK != ret) { // there has to be a macro for this right? - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - - ret = rmw_serialized_message_fini(&serialized_message); - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - - ret = rmw_serialized_message_fini(&serialized_message); - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - - // have to fini the request/response message here as well - rcl_interfaces__msg__ServiceEvent__fini(&msg); - - return ret; -} - rcl_ret_t rcl_send_response( const rcl_service_t * service, @@ -447,27 +407,17 @@ rcl_send_response( // publish out the introspected content send_introspection_message( service, - rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT; + rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, ros_response, request_header, - &options->allocator); + options); - - - if (rmw_send_response( service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - - // need a message form of ros_response as well. We have a response here but we want to be able to publish - // out a message as well. For now let's hardcode one in? - { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } return RCL_RET_OK; } diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h new file mode 100644 index 000000000..0c418c26c --- /dev/null +++ b/rcl/src/rcl/service_impl.h @@ -0,0 +1,32 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef RCL__SERVICE_IMPL_H_ +#define RCL__SERVICE_IMPL_H_ + + +#include "rcl/service.h" +#include "./introspection.h" + +struct rcl_service_impl_s +{ + rcl_service_options_t options; + rmw_qos_profile_t actual_request_subscription_qos; + rmw_qos_profile_t actual_response_publisher_qos; + rmw_service_t * rmw_handle; + rcl_service_introspection_utils_t * introspection_utils; +}; + +#endif // RCL__SERVICE_IMPL_H_ From 3a9b27deccf8ca1e31fc319a8258fbe065455568 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Mon, 27 Jun 2022 16:08:58 -0700 Subject: [PATCH 04/16] working demo with hardcoded typesupport Signed-off-by: Brian Chen --- rcl/src/rcl/introspection.c | 187 +++++++++++++++++++++++++++++++----- rcl/src/rcl/introspection.h | 29 +++++- rcl/src/rcl/service.c | 66 +++---------- 3 files changed, 200 insertions(+), 82 deletions(-) diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 6394e944c..26f2f7f1a 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -21,6 +21,7 @@ #include "rcl/error_handling.h" #include "rmw/error_handling.h" +#include "dlfcn.h" #include "rcutils/macros.h" #include "rcutils/logging_macros.h" @@ -33,18 +34,136 @@ #include "rcl_interfaces/msg/service_event_type.h" +rcl_service_introspection_utils_t +rcl_get_zero_initialized_introspection_utils () +{ + static rcl_service_introspection_utils_t null_introspection_utils = { + .response_type_support = NULL, + .request_type_support = NULL, + .publisher = NULL, + .clock = NULL, + .service_name = NULL + }; + return null_introspection_utils; +} + +rcl_ret_t +rcl_service_introspection_init( + rcl_service_introspection_utils_t * introspection_utils, + const rosidl_service_type_support_t * service_type_support, + const char * service_name, + const rcl_node_t * node, + rcl_allocator_t * allocator + ) +{ + + // the reason why we can't use a macro and concatenate the Type to get what we want is + // because there we had always {goal status result} but here we can have any service type name... + // TODO(ihasdapie): Need to get the package and message name somehow + // probably hook into rosidl + rcl_ret_t ret; + + introspection_utils->service_name = allocator->allocate( + strlen(service_name) + 1, allocator->state); + strcpy(introspection_utils->service_name, service_name); + + + + + char * tsfile = "libexample_interfaces__rosidl_typesupport_c.so"; + void* tslib = dlopen(tsfile, RTLD_LAZY); + RCL_CHECK_FOR_NULL_WITH_MSG(tslib, "dlopen failed", return RCL_RET_ERROR); + + + // there are macros for building these in rosidl_typesupport_interface but still need the msg package name + const char* ts_req_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Request"; + const char* ts_resp_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Response"; + + // TODO(ihasdapie): squash warning "ISO C forbids conversion of function pointer to object pointer type" + rosidl_message_type_support_t* (* ts_func_handle)() = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_resp_func_name); + RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", return RCL_RET_ERROR); + introspection_utils->response_type_support = ts_func_handle(); + ts_func_handle = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_req_func_name); + RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", return RCL_RET_ERROR); + introspection_utils->request_type_support = ts_func_handle(); + + char * service_event_name = (char*) allocator->zero_allocate( + strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, + sizeof(char), allocator->state); + strcpy(service_event_name, service_name); + strcat(service_event_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + + // Make a publisher + introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + *introspection_utils->publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * service_event_typesupport = + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); // ROSIDL_GET_MSG_TYPE_SUPPORT gives an int?? And complier warns that it is being converted to a ptr + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(introspection_utils->publisher, node, + service_event_typesupport, service_event_name, &publisher_options); + + + // there has to be a macro for this, right? + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + // make a clock + introspection_utils->clock = allocator->allocate(sizeof(rcl_clock_t), allocator->state); + ret = rcl_clock_init(RCL_STEADY_TIME, introspection_utils->clock, allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Clock initialized"); + + allocator->deallocate(service_event_name, allocator->state); + + return RCL_RET_OK; +} + + +rcl_ret_t +rcl_service_introspection_fini( + rcl_service_introspection_utils_t * introspection_utils, + rcl_allocator_t * allocator, + rcl_node_t * node) +{ + rcl_ret_t ret; + ret = rcl_publisher_fini(introspection_utils->publisher, node); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } + ret = rcl_clock_fini(introspection_utils->clock); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + allocator->deallocate(introspection_utils->publisher, allocator->state); + allocator->deallocate(introspection_utils->clock, allocator->state); + allocator->deallocate(introspection_utils->service_name, allocator->state); + + + return RCL_RET_OK; + +} + + rcl_ret_t -send_introspection_message( - const rcl_service_t * service, - uint8_t event_type, - void * ros_response_request, - rmw_request_id_t * header, +rcl_introspection_send_message( + const rcl_service_introspection_utils_t * introspection_utils, + const uint8_t event_type, + const void * ros_response_request, + const rmw_request_id_t * header, const rcl_service_options_t * options) { // need a copy of this function for request/response (?) and also in client.c unless there's // a utils.c or something... - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Sending introspection message -----------------"); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Sending introspection message -----------------"); rcl_ret_t ret; rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); @@ -54,24 +173,25 @@ send_introspection_message( RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message -----------------"); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message -----------------"); rcl_interfaces__msg__ServiceEvent msg; rcl_interfaces__msg__ServiceEvent__init(&msg); msg.event_type = event_type; msg.sequence_number = header->sequence_number; - rosidl_runtime_c__String__assign(&msg.service_name, rcl_service_get_service_name(service)); - printf("sequence_number: %ld\n", msg.sequence_number); + rosidl_runtime_c__String__assign(&msg.service_name, introspection_utils->service_name); + + /* printf("sequence_number: %ld\n", msg.sequence_number); printf("service_name: %s\n", msg.service_name.data); - printf("event_type: %d\n", msg.event_type); + printf("event_type: %d\n", msg.event_type); */ rosidl_runtime_c__String__assign(&msg.event_name, "AddTwoInts"); // TODO(ihasdapie): get this properly, the type name is hardcoded throughout right now - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building time message -----------------"); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building time message -----------------"); builtin_interfaces__msg__Time timestamp; builtin_interfaces__msg__Time__init(×tamp); rcl_time_point_value_t now; - ret = rcl_clock_get_now(service->impl->introspection_utils->clock, &now); + ret = rcl_clock_get_now(introspection_utils->clock, &now); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -81,27 +201,27 @@ send_introspection_message( timestamp.nanosec = now % (1000LL * 1000LL * 1000LL); msg.stamp = timestamp; - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building UUID message -----------------"); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building UUID message -----------------"); unique_identifier_msgs__msg__UUID uuid; unique_identifier_msgs__msg__UUID__init(&uuid); memcpy(uuid.uuid, header->writer_guid, 16 * sizeof(uint8_t)); msg.client_id = uuid; - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Serializing introspected message -----------------"); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Serializing introspected message -----------------"); rosidl_message_type_support_t * serialized_message_ts; switch (event_type) { case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: - serialized_message_ts = service->impl->introspection_utils->request_type_support; + serialized_message_ts = introspection_utils->request_type_support; break; case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: - serialized_message_ts = service->impl->introspection_utils->response_type_support; + serialized_message_ts = introspection_utils->response_type_support; break; case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED: - serialized_message_ts = service->impl->introspection_utils->response_type_support; + serialized_message_ts = introspection_utils->response_type_support; break; case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: - serialized_message_ts = service->impl->introspection_utils->request_type_support; + serialized_message_ts = introspection_utils->request_type_support; break; default: RCL_SET_ERROR_MSG("Invalid event type"); @@ -109,8 +229,8 @@ send_introspection_message( } ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: memcpying introspected message -----------------"); - printf("serialized_message.buffer_length: %zu, serialized_message size: %zu\n", serialized_message.buffer_length, serialized_message.buffer_capacity); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: memcpying introspected message -----------------"); + // printf("serialized_message.buffer_length: %zu, serialized_message size: %zu\n", serialized_message.buffer_length, serialized_message.buffer_capacity); rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); // This segfaults, but thought this should be ok? since serialized_message.buffer is a uint8_t* which _should_ be the same as a byte array @@ -119,14 +239,14 @@ send_introspection_message( memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Publishing ServiceEvent Message -----------------"); - ret = rcl_publish(service->impl->introspection_utils->publisher, &msg, NULL); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Publishing ServiceEvent Message -----------------"); + ret = rcl_publish(introspection_utils->publisher, &msg, NULL); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Cleanup -----------------"); + // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Send Introspection Message: Cleanup -----------------"); unique_identifier_msgs__msg__UUID__fini(&uuid); builtin_interfaces__msg__Time__fini(×tamp); @@ -141,4 +261,25 @@ send_introspection_message( } + + + + + + + + + + + + + + + + + + + + + #endif // RCL_INTROSPECTION_H_ diff --git a/rcl/src/rcl/introspection.h b/rcl/src/rcl/introspection.h index f3f1c5f84..ba4a5c812 100644 --- a/rcl/src/rcl/introspection.h +++ b/rcl/src/rcl/introspection.h @@ -30,15 +30,34 @@ typedef struct rcl_service_introspection_utils_s { rcl_publisher_t * publisher; rosidl_message_type_support_t * request_type_support; rosidl_message_type_support_t * response_type_support; + char * service_name; } rcl_service_introspection_utils_t; +rcl_service_introspection_utils_t +rcl_get_zero_initialized_introspection_utils(); + +rcl_ret_t +rcl_service_introspection_init( + rcl_service_introspection_utils_t * introspection_utils, + const rosidl_service_type_support_t * service_type_support, + const char * service_name, + const rcl_node_t * node, + rcl_allocator_t * allocator); + + + +rcl_ret_t +rcl_service_introspection_fini( + rcl_service_introspection_utils_t * introspection_utils, + rcl_allocator_t * allocator, + rcl_node_t * node); rcl_ret_t -send_introspection_message( - const rcl_service_t * service, - uint8_t event_type, - void * ros_response_request, - rmw_request_id_t * header, +rcl_introspection_send_message( + const rcl_service_introspection_utils_t * introspection_utils, + const uint8_t event_type, + const void * ros_response_request, + const rmw_request_id_t * header, const rcl_service_options_t * options); diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 221ba5011..b0b0cc606 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -94,8 +94,6 @@ rcl_service_init( // Expand and remap the given service name. char * remapped_service_name = NULL; - char * service_event_name = NULL; - // Preallocate space for for service_event_name rcl_ret_t ret = rcl_node_resolve_name( node, @@ -144,58 +142,21 @@ rcl_service_init( goto fail; } - // TODO(ihasdapie): Wrap this up as a fn introspection.c w/ `initialize_introspection_utils` and _fini, etc service->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( sizeof(rcl_service_introspection_utils_t), allocator->state); - // rcl_service_introspection_utils_t * iutils = service->impl->introspection_utils; - - // the reason why we can't use a macro and concatenate the Type to get what we want is - // because there we had always {goal status result} but here we can have any service type name... - // TODO(ihasdapie): Need to get the package and message name somehow - char * tsfile = "libexample_interfaces__rosidl_typesupport_c.so"; - void* tslib = dlopen(tsfile, RTLD_LAZY); - RCL_CHECK_FOR_NULL_WITH_MSG(tslib, "dlopen failed", ret = RCL_RET_ERROR; goto cleanup); - // there are macros for building these in rosidl_typesupport_interface but still need the msg package name - const char* ts_req_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Request"; - const char* ts_resp_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Response"; - - // TODO(ihasdapie): squash warning "ISO C forbids conversion of function pointer to object pointer type" - rosidl_message_type_support_t* (* ts_func_handle)() = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_resp_func_name); - RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", ret = RCL_RET_ERROR; goto cleanup); - service->impl->introspection_utils->response_type_support = ts_func_handle(); - ts_func_handle = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_req_func_name); - RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", ret = RCL_RET_ERROR; goto cleanup); - service->impl->introspection_utils->request_type_support = ts_func_handle(); - - service_event_name = (char*) allocator->zero_allocate( - strlen(remapped_service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, - sizeof(char), allocator->state); - strcpy(service_event_name, remapped_service_name); - strcat(service_event_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); - - // Make a publisher - service->impl->introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); - *service->impl->introspection_utils->publisher = rcl_get_zero_initialized_publisher(); - const rosidl_message_type_support_t * service_event_typesupport = - ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); // ROSIDL_GET_MSG_TYPE_SUPPORT gives an int?? And complier warns that it is being converted to a ptr - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(service->impl->introspection_utils->publisher, node, - service_event_typesupport, service_event_name, &publisher_options); - - // there has to be a macro for this, right? - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - goto fail; - } + *service->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); + ret = rcl_service_introspection_init( + service->impl->introspection_utils, + type_support, + remapped_service_name, + node, + allocator + ); - // make a clock - service->impl->introspection_utils->clock = allocator->allocate(sizeof(rcl_clock_t), allocator->state); - ret = rcl_clock_init(RCL_STEADY_TIME, service->impl->introspection_utils->clock, allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); goto fail; } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Clock initialized"); // get actual qos, and store it rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos( @@ -245,7 +206,6 @@ rcl_service_init( // Fall through to clean up cleanup: allocator->deallocate(remapped_service_name, allocator->state); - allocator->deallocate(service_event_name, allocator->state); return ret; } @@ -275,20 +235,18 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node); allocator.deallocate(service->impl->introspection_utils, allocator.state); + allocator.deallocate(service->impl, allocator.state); - service->impl->introspection_utils = NULL; service->impl = NULL; - ret = rcl_publisher_fini(service->impl->introspection_utils->publisher, node); - ret = rcl_clock_fini(service->impl->introspection_utils->clock); if (ret != RCL_RET_OK) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); result = RCL_RET_ERROR; } } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized"); return result; } @@ -405,8 +363,8 @@ rcl_send_response( RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); // publish out the introspected content - send_introspection_message( - service, + rcl_introspection_send_message( + service->impl->introspection_utils, rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, ros_response, request_header, From 23f5881cd0c7c4cbf73f692d4d14243e9556adeb Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Tue, 28 Jun 2022 11:38:28 -0700 Subject: [PATCH 05/16] try rosidl_service_introspection Signed-off-by: Brian Chen --- rcl/CMakeLists.txt | 2 ++ rcl/package.xml | 2 ++ rcl/src/rcl/introspection.c | 64 +++++++++++++++++++++++++++++-------- rcl/src/rcl/service.c | 1 - 4 files changed, 55 insertions(+), 14 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index b4891fa13..a6c7a7cd7 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -82,6 +82,7 @@ ament_target_dependencies(${PROJECT_NAME} ${RCL_LOGGING_IMPL} "rosidl_runtime_c" "tracetools" + "rosidl_typesupport_introspection_c" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -122,6 +123,7 @@ ament_export_dependencies(rcutils) ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(tracetools) +ament_export_dependencies(rosidl_typesupport_introspection_c) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rcl/package.xml b/rcl/package.xml index 03df8701b..564f0d66c 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -24,6 +24,8 @@ rosidl_runtime_c tracetools + rosidl_typesupport_introspection_c + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 26f2f7f1a..11c5cfe33 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -21,10 +21,12 @@ #include "rcl/error_handling.h" #include "rmw/error_handling.h" + #include "dlfcn.h" #include "rcutils/macros.h" #include "rcutils/logging_macros.h" +#include "rcutils/shared_library.h" #include "rosidl_runtime_c/string_functions.h" #include "rosidl_runtime_c/primitives_sequence_functions.h" @@ -33,6 +35,11 @@ #include "rcl_interfaces/msg/service_event.h" #include "rcl_interfaces/msg/service_event_type.h" +#include "rosidl_typesupport_introspection_c/service_introspection.h" + +#include "rosidl_typesupport_introspection_c/identifier.h" + + rcl_service_introspection_utils_t rcl_get_zero_initialized_introspection_utils () @@ -69,23 +76,54 @@ rcl_service_introspection_init( + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Typesupport -----------------"); + + printf("Service typesupport identifier: %s\n", service_type_support->typesupport_identifier); + + const rosidl_service_type_support_t * introspection_type_support = get_service_typesupport_handle( + service_type_support, + // "rosidl_typesupport_introspection_cpp" // this will "work" and then segfault when trying to serialize + rosidl_typesupport_introspection_c__identifier); + + + if (!introspection_type_support) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get introspection typesupport handle"); + return RCL_RET_ERROR; + } + + // printf("Typesupport identifier: %s\n", introspection_type_support->typesupport_identifier); + + + + rosidl_typesupport_introspection_c__ServiceMembers * members; + + if (strcmp(introspection_type_support->typesupport_identifier, "rosidl_typesupport_introspection_c") == 0) { + members = (rosidl_typesupport_introspection_c__ServiceMembers *) introspection_type_support->data; + + } else if (strcmp(introspection_type_support->typesupport_identifier, "rosidl_typesupport_introspection_cpp") == 0) { + // TODO(ihasdapie): Grabbing cpp members (?) + /* ServiceMembers * members = + (ServiceMembers *) introspection_type_support->data; */ + members = (rosidl_typesupport_introspection_c__ServiceMembers *) introspection_type_support->data; + } + else { + printf("Unknown typesupport identifier: %s\n", introspection_type_support->typesupport_identifier); + return RCL_RET_ERROR; + // unknown ts + } + + const rosidl_message_type_support_t * request_ts = members->request_members_->members_->members_; + const rosidl_message_type_support_t * response_ts = members->response_members_->members_->members_; + introspection_utils->request_type_support = request_ts; + introspection_utils->response_type_support = response_ts; + + + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Typesupport DONE -----------------"); + - char * tsfile = "libexample_interfaces__rosidl_typesupport_c.so"; - void* tslib = dlopen(tsfile, RTLD_LAZY); - RCL_CHECK_FOR_NULL_WITH_MSG(tslib, "dlopen failed", return RCL_RET_ERROR); - // there are macros for building these in rosidl_typesupport_interface but still need the msg package name - const char* ts_req_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Request"; - const char* ts_resp_func_name = "rosidl_typesupport_c__get_message_type_support_handle__example_interfaces__srv__AddTwoInts_Response"; - // TODO(ihasdapie): squash warning "ISO C forbids conversion of function pointer to object pointer type" - rosidl_message_type_support_t* (* ts_func_handle)() = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_resp_func_name); - RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", return RCL_RET_ERROR); - introspection_utils->response_type_support = ts_func_handle(); - ts_func_handle = (rosidl_message_type_support_t*(*)()) dlsym(tslib, ts_req_func_name); - RCL_CHECK_FOR_NULL_WITH_MSG(ts_func_handle, "looking up response type support failed", return RCL_RET_ERROR); - introspection_utils->request_type_support = ts_func_handle(); char * service_event_name = (char*) allocator->zero_allocate( strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index b0b0cc606..f3d3b21e5 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -200,7 +200,6 @@ rcl_service_init( allocator->deallocate(service->impl, allocator->state); service->impl->introspection_utils = NULL; service->impl = NULL; - // iutils= NULL; } ret = fail_ret; // Fall through to clean up From 9c0b42edace34ff9783274e4eb18ceeaacc87015 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 29 Jun 2022 16:10:33 -0700 Subject: [PATCH 06/16] service introspection hooked into all four service events Signed-off-by: Brian Chen --- rcl/CMakeLists.txt | 4 + rcl/include/rcl/service.h | 10 ++ rcl/package.xml | 1 + rcl/src/rcl/client.c | 40 +++++ rcl/src/rcl/introspection.c | 305 +++++++++++++++++------------------- rcl/src/rcl/introspection.h | 38 +++-- rcl/src/rcl/service.c | 60 +++++-- 7 files changed, 267 insertions(+), 191 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index a6c7a7cd7..4293a1730 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -12,6 +12,8 @@ find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rosidl_runtime_c REQUIRED) find_package(tracetools REQUIRED) +find_package(rosidl_typesupport_introspection_c) +find_package(rosidl_typesupport_c) include(cmake/rcl_set_symbol_visibility_hidden.cmake) include(cmake/get_default_rcl_logging_implementation.cmake) @@ -83,6 +85,7 @@ ament_target_dependencies(${PROJECT_NAME} "rosidl_runtime_c" "tracetools" "rosidl_typesupport_introspection_c" + "rosidl_typesupport_c" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -124,6 +127,7 @@ ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(tracetools) ament_export_dependencies(rosidl_typesupport_introspection_c) +ament_export_dependencies(rosidl_typesupport_c) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 2461bd551..f30187358 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -361,6 +361,16 @@ RCL_WARN_UNUSED const char * rcl_service_get_service_name(const rcl_service_t * service); +/// Get the service type name for the service +/** + + * TODO(ihasdapie): document this function + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_service_get_service_type_name(const rosidl_service_type_support_t * service_type_support); + /// Return the rcl service options. /** * This function returns the service's internal options struct. diff --git a/rcl/package.xml b/rcl/package.xml index 564f0d66c..18da61d6e 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -25,6 +25,7 @@ tracetools rosidl_typesupport_introspection_c + rosidl_typesupport_c ament_cmake_gtest ament_lint_auto diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 432948d59..a5fbb7399 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -30,7 +30,9 @@ extern "C" #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "tracetools/tracetools.h" +#include "rcl_interfaces/msg/service_event_type.h" +#include "./introspection.h" #include "./common.h" struct rcl_client_impl_s @@ -40,6 +42,7 @@ struct rcl_client_impl_s rmw_qos_profile_t actual_response_subscription_qos; rmw_client_t * rmw_handle; atomic_int_least64_t sequence_number; + rcl_service_introspection_utils_t * introspection_utils; }; rcl_client_t @@ -114,6 +117,14 @@ rcl_client_init( goto fail; } + client->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( + sizeof(rcl_service_introspection_utils_t), allocator->state); + + *client->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); + ret = rcl_service_introspection_init( + client->impl->introspection_utils, type_support, + remapped_service_name, node, allocator); + // get actual qos, and store it rmw_ret_t rmw_ret = rmw_client_request_publisher_get_actual_qos( client->impl->rmw_handle, @@ -252,6 +263,22 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t return RCL_RET_ERROR; } rcutils_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number); + + // TODO(ihasdapie): Writ + uint8_t tmp_writer_guid[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + rcl_ret_t rclret = rcl_introspection_send_message( + client->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__REQUEST_SENT, + ros_request, + *sequence_number, + tmp_writer_guid, + // request_header->request_id.writer_guid, + &client->impl->options.allocator); + + if (RCL_RET_OK != rclret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } return RCL_RET_OK; } @@ -283,6 +310,19 @@ rcl_take_response_with_info( if (!taken) { return RCL_RET_CLIENT_TAKE_FAILED; } + + rcl_ret_t rclret = rcl_introspection_send_message( + client->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED, + ros_response, + request_header->request_id.sequence_number, + request_header->request_id.writer_guid, + &client->impl->options.allocator); + + if (RCL_RET_OK != rclret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } return RCL_RET_OK; } diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 11c5cfe33..93cdbfbd3 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -12,11 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef RCL_INTROSPECTION_H_ #define RCL_INTROSPECTION_H_ - #include "./service_impl.h" #include "rcl/error_handling.h" @@ -24,124 +22,145 @@ #include "dlfcn.h" -#include "rcutils/macros.h" #include "rcutils/logging_macros.h" +#include "rcutils/macros.h" #include "rcutils/shared_library.h" -#include "rosidl_runtime_c/string_functions.h" #include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rosidl_runtime_c/string_functions.h" -#include "unique_identifier_msgs/msg/uuid.h" #include "builtin_interfaces/msg/time.h" #include "rcl_interfaces/msg/service_event.h" #include "rcl_interfaces/msg/service_event_type.h" +#include "rcl_interfaces/msg/service_event_info.h" +#include "unique_identifier_msgs/msg/uuid.h" #include "rosidl_typesupport_introspection_c/service_introspection.h" - +#include "rosidl_typesupport_c/type_support_map.h" #include "rosidl_typesupport_introspection_c/identifier.h" - - +#include rcl_service_introspection_utils_t -rcl_get_zero_initialized_introspection_utils () +rcl_get_zero_initialized_introspection_utils() { static rcl_service_introspection_utils_t null_introspection_utils = { .response_type_support = NULL, .request_type_support = NULL, .publisher = NULL, .clock = NULL, - .service_name = NULL + .service_name = NULL, + .service_type_name = NULL, }; return null_introspection_utils; } + rcl_ret_t -rcl_service_introspection_init( - rcl_service_introspection_utils_t * introspection_utils, - const rosidl_service_type_support_t * service_type_support, - const char * service_name, - const rcl_node_t * node, - rcl_allocator_t * allocator - ) +rcl_service_typesupport_to_message_typesupport( + const rosidl_service_type_support_t * service_typesupport, + rosidl_message_type_support_t ** request_typesupport, + rosidl_message_type_support_t ** response_typesupport, + const rcl_allocator_t * allocator) { - - // the reason why we can't use a macro and concatenate the Type to get what we want is - // because there we had always {goal status result} but here we can have any service type name... - // TODO(ihasdapie): Need to get the package and message name somehow - // probably hook into rosidl - rcl_ret_t ret; - - introspection_utils->service_name = allocator->allocate( - strlen(service_name) + 1, allocator->state); - strcpy(introspection_utils->service_name, service_name); - + rcutils_ret_t ret; + type_support_map_t * map = (type_support_map_t *)service_typesupport->data; + const char * typesupport_library_fmt = "lib%s__rosidl_typesupport_c.so"; + const char * service_message_fmt = // package_name, type name, Request/Response + "rosidl_typesupport_c__get_message_type_support_handle__%s__srv__%s_%s"; - - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Typesupport -----------------"); - - printf("Service typesupport identifier: %s\n", service_type_support->typesupport_identifier); - - const rosidl_service_type_support_t * introspection_type_support = get_service_typesupport_handle( - service_type_support, - // "rosidl_typesupport_introspection_cpp" // this will "work" and then segfault when trying to serialize - rosidl_typesupport_introspection_c__identifier); + const char * service_type_name = rcl_service_get_service_type_name(service_typesupport); - - if (!introspection_type_support) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get introspection typesupport handle"); - return RCL_RET_ERROR; - } - - // printf("Typesupport identifier: %s\n", introspection_type_support->typesupport_identifier); - - - - rosidl_typesupport_introspection_c__ServiceMembers * members; - - if (strcmp(introspection_type_support->typesupport_identifier, "rosidl_typesupport_introspection_c") == 0) { - members = (rosidl_typesupport_introspection_c__ServiceMembers *) introspection_type_support->data; - - } else if (strcmp(introspection_type_support->typesupport_identifier, "rosidl_typesupport_introspection_cpp") == 0) { - // TODO(ihasdapie): Grabbing cpp members (?) - /* ServiceMembers * members = - (ServiceMembers *) introspection_type_support->data; */ - members = (rosidl_typesupport_introspection_c__ServiceMembers *) introspection_type_support->data; - } - else { - printf("Unknown typesupport identifier: %s\n", introspection_type_support->typesupport_identifier); + // build out typesupport library and symbol names + char * typesupport_library_name = allocator->allocate( + sizeof(char) * ((strlen(typesupport_library_fmt) - 2) + + strlen(map->package_name) + 1), + allocator->state); + char * request_message_symbol = allocator->allocate( + sizeof(char) * + ((strlen(service_message_fmt) - 6) + strlen(map->package_name) + + strlen(service_type_name) + strlen("Request") + 1), + allocator->state); + char * response_message_symbol = allocator->allocate( + sizeof(char) * + ((strlen(service_message_fmt) - 6) + strlen(map->package_name) + + strlen(service_type_name) + strlen("Request") + 1), + allocator->state); + + sprintf(typesupport_library_name, typesupport_library_fmt, map->package_name); + sprintf(request_message_symbol, service_message_fmt, map->package_name, + service_type_name, "Request"); + sprintf(response_message_symbol, service_message_fmt, map->package_name, + service_type_name, "Response"); + + rcutils_shared_library_t typesupport_library = rcutils_get_zero_initialized_shared_library(); + ret = rcutils_load_shared_library(&typesupport_library, typesupport_library_name, *allocator); + if (RCUTILS_RET_OK != ret) { return RCL_RET_ERROR; - // unknown ts } - const rosidl_message_type_support_t * request_ts = members->request_members_->members_->members_; - const rosidl_message_type_support_t * response_ts = members->response_members_->members_->members_; - introspection_utils->request_type_support = request_ts; - introspection_utils->response_type_support = response_ts; + rosidl_message_type_support_t *(*req_typesupport_func_handle)() = + (rosidl_message_type_support_t * (*)()) + rcutils_get_symbol(&typesupport_library, request_message_symbol); + RCL_CHECK_FOR_NULL_WITH_MSG(req_typesupport_func_handle, + "Looking up request type support failed", return RCL_RET_ERROR); + rosidl_message_type_support_t *(*resp_typesupport_func_handle)() = + (rosidl_message_type_support_t * (*)()) + rcutils_get_symbol(&typesupport_library, response_message_symbol); + RCL_CHECK_FOR_NULL_WITH_MSG(resp_typesupport_func_handle, + "Looking up response type support failed", return RCL_RET_ERROR); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Typesupport DONE -----------------"); + *request_typesupport = req_typesupport_func_handle(); + *response_typesupport = resp_typesupport_func_handle(); + allocator->deallocate(typesupport_library_name, allocator->state); + allocator->deallocate(request_message_symbol, allocator->state); + allocator->deallocate(response_message_symbol, allocator->state); + return RCL_RET_OK; +} +rcl_ret_t rcl_service_introspection_init( + rcl_service_introspection_utils_t * introspection_utils, + const rosidl_service_type_support_t * service_type_support, + const char * service_name, + const rcl_node_t * node, + rcl_allocator_t * allocator) +{ + rcl_ret_t ret; + introspection_utils->service_name = allocator->allocate( + strlen(service_name) + 1, allocator->state); + strcpy(introspection_utils->service_name, service_name); + const char* service_type_name = rcl_service_get_service_type_name(service_type_support); + introspection_utils->service_type_name = allocator->allocate( + strlen(service_type_name) + 1, allocator->state); + strcpy(introspection_utils->service_type_name, service_type_name); + rcl_service_typesupport_to_message_typesupport( + service_type_support, &introspection_utils->request_type_support, + &introspection_utils->response_type_support, allocator); - char * service_event_name = (char*) allocator->zero_allocate( - strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, - sizeof(char), allocator->state); + // Make a publisher + char * service_event_name = (char *)allocator->zero_allocate( + strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, + sizeof(char), allocator->state); strcpy(service_event_name, service_name); strcat(service_event_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); - // Make a publisher - introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + introspection_utils->publisher = + allocator->allocate(sizeof(rcl_publisher_t), allocator->state); *introspection_utils->publisher = rcl_get_zero_initialized_publisher(); - const rosidl_message_type_support_t * service_event_typesupport = - ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); // ROSIDL_GET_MSG_TYPE_SUPPORT gives an int?? And complier warns that it is being converted to a ptr - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(introspection_utils->publisher, node, - service_event_typesupport, service_event_name, &publisher_options); - - - // there has to be a macro for this, right? + const rosidl_message_type_support_t * service_event_typesupport = + ROSIDL_GET_MSG_TYPE_SUPPORT( + rcl_interfaces, msg, + ServiceEvent); + + rcl_publisher_options_t publisher_options = + rcl_publisher_get_default_options(); + ret = rcl_publisher_init( + introspection_utils->publisher, node, + service_event_typesupport, service_event_name, + &publisher_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); return RCL_RET_ERROR; @@ -154,19 +173,16 @@ rcl_service_introspection_init( RCL_SET_ERROR_MSG(rcl_get_error_string().str); return RCL_RET_ERROR; } - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Clock initialized"); allocator->deallocate(service_event_name, allocator->state); return RCL_RET_OK; } - rcl_ret_t rcl_service_introspection_fini( rcl_service_introspection_utils_t * introspection_utils, - rcl_allocator_t * allocator, - rcl_node_t * node) + rcl_allocator_t * allocator, rcl_node_t * node) { rcl_ret_t ret; ret = rcl_publisher_fini(introspection_utils->publisher, node); @@ -184,69 +200,29 @@ rcl_service_introspection_fini( allocator->deallocate(introspection_utils->clock, allocator->state); allocator->deallocate(introspection_utils->service_name, allocator->state); - return RCL_RET_OK; - } - - -rcl_ret_t +rcl_ret_t rcl_introspection_send_message( const rcl_service_introspection_utils_t * introspection_utils, const uint8_t event_type, const void * ros_response_request, - const rmw_request_id_t * header, - const rcl_service_options_t * options) + const int64_t sequence_number, + const uint8_t uuid[16], + const rcl_allocator_t * allocator) { - // need a copy of this function for request/response (?) and also in client.c unless there's - // a utils.c or something... - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Sending introspection message -----------------"); - rcl_ret_t ret; - rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); - - ret = rmw_serialized_message_init(&serialized_message, 0u, &options->allocator); // why is this 0u? - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message -----------------"); + rcl_interfaces__msg__ServiceEvent msg; rcl_interfaces__msg__ServiceEvent__init(&msg); - msg.event_type = event_type; - msg.sequence_number = header->sequence_number; - rosidl_runtime_c__String__assign(&msg.service_name, introspection_utils->service_name); - - /* printf("sequence_number: %ld\n", msg.sequence_number); - printf("service_name: %s\n", msg.service_name.data); - printf("event_type: %d\n", msg.event_type); */ - - rosidl_runtime_c__String__assign(&msg.event_name, "AddTwoInts"); // TODO(ihasdapie): get this properly, the type name is hardcoded throughout right now - - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building time message -----------------"); - builtin_interfaces__msg__Time timestamp; - builtin_interfaces__msg__Time__init(×tamp); - - rcl_time_point_value_t now; - ret = rcl_clock_get_now(introspection_utils->clock, &now); - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - // is there a rcl method for getting the seconds too? - timestamp.sec = RCL_NS_TO_S(now); - timestamp.nanosec = now % (1000LL * 1000LL * 1000LL); - msg.stamp = timestamp; - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building UUID message -----------------"); - unique_identifier_msgs__msg__UUID uuid; - unique_identifier_msgs__msg__UUID__init(&uuid); - memcpy(uuid.uuid, header->writer_guid, 16 * sizeof(uint8_t)); - msg.client_id = uuid; - - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Serializing introspected message -----------------"); + rcl_interfaces__msg__ServiceEventInfo msg_info; + rcl_interfaces__msg__ServiceEventInfo__init(&msg_info); + // build serialized message + rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); + ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); rosidl_message_type_support_t * serialized_message_ts; switch (event_type) { case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: @@ -267,57 +243,56 @@ rcl_introspection_send_message( } ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: memcpying introspected message -----------------"); - // printf("serialized_message.buffer_length: %zu, serialized_message size: %zu\n", serialized_message.buffer_length, serialized_message.buffer_capacity); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); + memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); - // This segfaults, but thought this should be ok? since serialized_message.buffer is a uint8_t* which _should_ be the same as a byte array - // rosidl_runtime_c__octet__Sequence__copy((rosidl_runtime_c__octet__Sequence *) serialized_message.buffer, &msg.serialized_event); + // populate info message - memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); + msg_info.event_type = event_type; + msg_info.sequence_number = sequence_number; + rosidl_runtime_c__String__assign(&msg_info.service_name, introspection_utils->service_name); + rosidl_runtime_c__String__assign(&msg_info.event_name, introspection_utils->service_type_name); + builtin_interfaces__msg__Time timestamp; + builtin_interfaces__msg__Time__init(×tamp); - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Publishing ServiceEvent Message -----------------"); - ret = rcl_publish(introspection_utils->publisher, &msg, NULL); + rcl_time_point_value_t now; + ret = rcl_clock_get_now(introspection_utils->clock, &now); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + timestamp.sec = RCL_NS_TO_S(now); + timestamp.nanosec = now % (1000LL * 1000LL * 1000LL); + msg_info.stamp = timestamp; - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Send Introspection Message: Cleanup -----------------"); + unique_identifier_msgs__msg__UUID uuid_msg; + unique_identifier_msgs__msg__UUID__init(&uuid_msg); + memcpy(uuid_msg.uuid, uuid, 16 * sizeof(uint8_t)); + msg_info.client_id = uuid_msg; + + // and publish it out! + ret = rcl_publish(introspection_utils->publisher, &msg, NULL); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } - unique_identifier_msgs__msg__UUID__fini(&uuid); + // clean up + unique_identifier_msgs__msg__UUID__fini(&uuid_msg); builtin_interfaces__msg__Time__fini(×tamp); ret = rmw_serialized_message_fini(&serialized_message); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - // have to fini the request/response message here as well + rcl_interfaces__msg__ServiceEventInfo__fini(&msg_info); rcl_interfaces__msg__ServiceEvent__fini(&msg); return RCL_RET_OK; } - - - - - - - - - - - - - - - - - - - - - - #endif // RCL_INTROSPECTION_H_ diff --git a/rcl/src/rcl/introspection.h b/rcl/src/rcl/introspection.h index ba4a5c812..05bec1d26 100644 --- a/rcl/src/rcl/introspection.h +++ b/rcl/src/rcl/introspection.h @@ -12,17 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef RCL__SERVICE_INTROSPECTION_H_ #define RCL__SERVICE_INTROSPECTION_H_ -#include "rcl/time.h" #include "rcl/publisher.h" #include "rcl/service.h" +#include "rcl/time.h" #include "rmw/rmw.h" - - #define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" typedef struct rcl_service_introspection_utils_s { @@ -31,11 +28,16 @@ typedef struct rcl_service_introspection_utils_s { rosidl_message_type_support_t * request_type_support; rosidl_message_type_support_t * response_type_support; char * service_name; + char * service_type_name; } rcl_service_introspection_utils_t; +RCL_PUBLIC +RCL_WARN_UNUSED rcl_service_introspection_utils_t rcl_get_zero_initialized_introspection_utils(); +RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_service_introspection_init( rcl_service_introspection_utils_t * introspection_utils, @@ -44,21 +46,37 @@ rcl_service_introspection_init( const rcl_node_t * node, rcl_allocator_t * allocator); - - +RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_service_introspection_fini( rcl_service_introspection_utils_t * introspection_utils, rcl_allocator_t * allocator, - rcl_node_t * node); + rcl_node_t * node); -rcl_ret_t +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_introspection_send_message( const rcl_service_introspection_utils_t * introspection_utils, const uint8_t event_type, const void * ros_response_request, - const rmw_request_id_t * header, - const rcl_service_options_t * options); + const int64_t sequence_number, + const uint8_t uuid[16], // uuid is uint8_t but the guid is int8_t + const rcl_allocator_t * allocator); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_disable(rcl_service_t * service); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_enable(rcl_service_t * service); + #endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index f3d3b21e5..d8e8685a5 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -17,7 +17,6 @@ extern "C" { #endif -#include #include #include @@ -43,7 +42,8 @@ extern "C" #include "rosidl_runtime_c/message_type_support_struct.h" #include "rcl_interfaces/msg/service_event_type.h" -#include "rcl_interfaces/msg/service_event.h" + +#include "rosidl_typesupport_c/type_support_map.h" @@ -234,7 +234,11 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node); + ret = rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } allocator.deallocate(service->impl->introspection_utils, allocator.state); allocator.deallocate(service->impl, allocator.state); @@ -274,6 +278,18 @@ rcl_service_get_service_name(const rcl_service_t * service) #define _service_get_options(service) & service->impl->options +const char * +rcl_service_get_service_type_name(const rosidl_service_type_support_t * service_type_support) +{ + type_support_map_t * map = (type_support_map_t *)service_type_support->data; + // By inspection all of the symbol_name(s) end in the service type name + // We can do this because underscores not allowed in service/msg/action names + char* service_type_name = strrchr(map->symbol_name[0], '_'); + service_type_name++; + return service_type_name; +} + + const rcl_service_options_t * rcl_service_get_options(const rcl_service_t * service) { @@ -307,12 +323,6 @@ rcl_take_request_with_info( const rcl_service_options_t * options = rcl_service_get_options(service); RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); - /* send_introspection_message( - service, - rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED, - ros_request, - request_header, - options); */ bool taken = false; rmw_ret_t ret = rmw_take_request( @@ -329,6 +339,18 @@ rcl_take_request_with_info( if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } + + rcl_ret_t rclret = rcl_introspection_send_message( + service->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED, + ros_request, + request_header->request_id.sequence_number, + request_header->request_id.writer_guid, + &options->allocator); + if (RCL_RET_OK != rclret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } return RCL_RET_OK; } @@ -361,13 +383,6 @@ rcl_send_response( const rcl_service_options_t * options = rcl_service_get_options(service); RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); - // publish out the introspected content - rcl_introspection_send_message( - service->impl->introspection_utils, - rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, - ros_response, - request_header, - options); if (rmw_send_response( service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK) @@ -375,6 +390,19 @@ rcl_send_response( RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + + // publish out the introspected content + rcl_ret_t ret = rcl_introspection_send_message( + service->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, + ros_response, + request_header->sequence_number, + request_header->writer_guid, + &options->allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } return RCL_RET_OK; } From 6dca2e8a549eb7dd74e3539094ceaef165e364e1 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 6 Jul 2022 17:37:29 -0700 Subject: [PATCH 07/16] use new rmw_client_t.writer_guid Signed-off-by: Brian Chen --- rcl/src/rcl/client.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index a5fbb7399..b9395d736 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -264,15 +264,12 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t } rcutils_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number); - // TODO(ihasdapie): Writ - uint8_t tmp_writer_guid[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; rcl_ret_t rclret = rcl_introspection_send_message( client->impl->introspection_utils, rcl_interfaces__msg__ServiceEventType__REQUEST_SENT, ros_request, *sequence_number, - tmp_writer_guid, - // request_header->request_id.writer_guid, + client->impl->rmw_handle->writer_guid, &client->impl->options.allocator); if (RCL_RET_OK != rclret) { @@ -316,7 +313,7 @@ rcl_take_response_with_info( rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED, ros_response, request_header->request_id.sequence_number, - request_header->request_id.writer_guid, + client->impl->rmw_handle->writer_guid, &client->impl->options.allocator); if (RCL_RET_OK != rclret) { From 652cff936ce108480dddfa06d29df81141d8352b Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 6 Jul 2022 17:37:55 -0700 Subject: [PATCH 08/16] use correct typesupport and actually populate message info field Signed-off-by: Brian Chen --- rcl/src/rcl/introspection.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 93cdbfbd3..9a5cbbf51 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -225,23 +225,24 @@ rcl_introspection_send_message( ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); rosidl_message_type_support_t * serialized_message_ts; switch (event_type) { + case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: + serialized_message_ts = introspection_utils->request_type_support; + break; case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: serialized_message_ts = introspection_utils->request_type_support; break; - case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: + case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: serialized_message_ts = introspection_utils->response_type_support; break; case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED: serialized_message_ts = introspection_utils->response_type_support; break; - case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: - serialized_message_ts = introspection_utils->request_type_support; - break; default: RCL_SET_ERROR_MSG("Invalid event type"); return RCL_RET_ERROR; } + ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); @@ -251,7 +252,6 @@ rcl_introspection_send_message( memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); // populate info message - msg_info.event_type = event_type; msg_info.sequence_number = sequence_number; rosidl_runtime_c__String__assign(&msg_info.service_name, introspection_utils->service_name); @@ -275,6 +275,8 @@ rcl_introspection_send_message( memcpy(uuid_msg.uuid, uuid, 16 * sizeof(uint8_t)); msg_info.client_id = uuid_msg; + msg.info = msg_info; + // and publish it out! ret = rcl_publish(introspection_utils->publisher, &msg, NULL); if (RMW_RET_OK != ret) { @@ -290,8 +292,8 @@ rcl_introspection_send_message( RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - rcl_interfaces__msg__ServiceEventInfo__fini(&msg_info); - rcl_interfaces__msg__ServiceEvent__fini(&msg); + // rcl_interfaces__msg__ServiceEventInfo__fini(&msg_info); + // rcl_interfaces__msg__ServiceEvent__fini(&msg); return RCL_RET_OK; } From 3c8eb7589ddc7dbf28d010d722a483a81d24ecd1 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 7 Jul 2022 16:50:00 -0700 Subject: [PATCH 09/16] move rcl_client_impl_s into client_impl.h to improve ergonomics for service introspection Signed-off-by: Brian Chen --- rcl/src/rcl/client.c | 16 ++++------------ rcl/src/rcl/client_impl.h | 35 +++++++++++++++++++++++++++++++++++ 2 files changed, 39 insertions(+), 12 deletions(-) create mode 100644 rcl/src/rcl/client_impl.h diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index b9395d736..70df329b4 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -26,24 +26,16 @@ extern "C" #include "rcl/node.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" -#include "rcutils/stdatomic_helper.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "tracetools/tracetools.h" #include "rcl_interfaces/msg/service_event_type.h" -#include "./introspection.h" -#include "./common.h" -struct rcl_client_impl_s -{ - rcl_client_options_t options; - rmw_qos_profile_t actual_request_publisher_qos; - rmw_qos_profile_t actual_response_subscription_qos; - rmw_client_t * rmw_handle; - atomic_int_least64_t sequence_number; - rcl_service_introspection_utils_t * introspection_utils; -}; +#include "rcl/introspection.h" + +#include "./common.h" +#include "./client_impl.h" rcl_client_t rcl_get_zero_initialized_client() diff --git a/rcl/src/rcl/client_impl.h b/rcl/src/rcl/client_impl.h new file mode 100644 index 000000000..8adc64208 --- /dev/null +++ b/rcl/src/rcl/client_impl.h @@ -0,0 +1,35 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef RCL__CLIENT_IMPL_H_ +#define RCL__CLIENT_IMPL_H_ + + +#include "rcl/client.h" + +#include "rcutils/stdatomic_helper.h" +#include "rcl/introspection.h" + +struct rcl_client_impl_s +{ + rcl_client_options_t options; + rmw_qos_profile_t actual_request_publisher_qos; + rmw_qos_profile_t actual_response_subscription_qos; + rmw_client_t * rmw_handle; + atomic_int_least64_t sequence_number; + rcl_service_introspection_utils_t * introspection_utils; +}; + +#endif // RCL__CLIENT_IMPL_H_ From 3fd6e2c1d9041994478bc3fcec5d7af3986acbd5 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 7 Jul 2022 16:52:19 -0700 Subject: [PATCH 10/16] move src/introspection.h to include/introspection.h Signed-off-by: Brian Chen --- rcl/include/rcl/introspection.h | 153 ++++++++++++++++++++++++++++++-- rcl/src/rcl/introspection.c | 13 ++- rcl/src/rcl/introspection.h | 82 ----------------- rcl/src/rcl/service.c | 4 +- rcl/src/rcl/service_impl.h | 3 +- 5 files changed, 162 insertions(+), 93 deletions(-) delete mode 100644 rcl/src/rcl/introspection.h diff --git a/rcl/include/rcl/introspection.h b/rcl/include/rcl/introspection.h index 7cb010e22..9060ffd62 100644 --- a/rcl/include/rcl/introspection.h +++ b/rcl/include/rcl/introspection.h @@ -12,14 +12,155 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef RCL__SERVICE_INTROSPECTION_H_ +#define RCL__SERVICE_INTROSPECTION_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/publisher.h" #include "rcl/service.h" -#include "rcl/macros.h" +#include "rcl/time.h" +#include "rmw/rmw.h" +#include "rcl/client.h" + +#define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" + +typedef struct rcl_service_introspection_utils_s { + rcl_clock_t * clock; + rcl_publisher_t * publisher; + rosidl_message_type_support_t * request_type_support; + rosidl_message_type_support_t * response_type_support; + char * service_name; + char * service_type_name; + char * service_event_topic_name; + bool _enabled; + bool _content_enabled; +} rcl_service_introspection_utils_t; + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_introspection_utils_t +rcl_get_zero_initialized_introspection_utils(); RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t -send_introspection_message( - rcl_service_t * service, - void * service_payload, - rmw_request_id_t * header, +rcl_ret_t +rcl_service_introspection_init( + rcl_service_introspection_utils_t * introspection_utils, + const rosidl_service_type_support_t * service_type_support, + const char * service_name, + const rcl_node_t * node, rcl_allocator_t * allocator); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_fini( + rcl_service_introspection_utils_t * introspection_utils, + rcl_allocator_t * allocator, + rcl_node_t * node); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_introspection_send_message( + const rcl_service_introspection_utils_t * introspection_utils, + const uint8_t event_type, + const void * ros_response_request, + const int64_t sequence_number, + const uint8_t uuid[16], // uuid is uint8_t but the guid is int8_t + const rcl_allocator_t * allocator); + + + + + +/* Enables service introspection by reconstructing the introspection clock and publisher + * + * Does nothing and returns RCL_RET_OK if already enabled + */ +RCL_LOCAL +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_enable( + rcl_service_introspection_utils_t * introspection_utils, + const rcl_node_t * node, + rcl_allocator_t * allocator); + +/* Disabled service introspection by fini-ing and freeing the introspection clock and publisher + * + * Does nothing and returns RCL_RET_OK if already disabled + * + * + * + */ + +RCL_LOCAL +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_disable( + rcl_service_introspection_utils_t * introspection_utils, + rcl_node_t * node, + const rcl_allocator_t * allocator); + + + + +/* + * Enables/disables service introspection for client/service + * These functions are thin wrappers around rcl_service_introspection_{enable, disable} + * + * + * + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_enable_service_events(rcl_service_t * service, rcl_node_t * node); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_disable_service_events(rcl_service_t * service, rcl_node_t * node); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_enable_client_events(rcl_client_t * client, rcl_node_t * node); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_t * node); + + +/* + * + */ +RCL_PUBLIC +void +rcl_service_introspection_configure_client_event_content(rcl_client_t * client, bool enable); + +RCL_PUBLIC +void +rcl_service_introspection_configure_service_event_content(rcl_service_t * service, bool enable); + + +// TODO(ihasdapie): Do we want some getters for if content and/or introspection is enabled/disabled? + + + + + + + + +#ifdef __cplusplus +} +#endif + + +#endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 9a5cbbf51..f2230dc0c 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCL_INTROSPECTION_H_ -#define RCL_INTROSPECTION_H_ +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/introspection.h" #include "./service_impl.h" +#include "./client_impl.h" #include "rcl/error_handling.h" #include "rmw/error_handling.h" @@ -297,4 +302,6 @@ rcl_introspection_send_message( return RCL_RET_OK; } -#endif // RCL_INTROSPECTION_H_ +#ifdef __cplusplus +} +#endif diff --git a/rcl/src/rcl/introspection.h b/rcl/src/rcl/introspection.h deleted file mode 100644 index 05bec1d26..000000000 --- a/rcl/src/rcl/introspection.h +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright 2022 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCL__SERVICE_INTROSPECTION_H_ -#define RCL__SERVICE_INTROSPECTION_H_ - -#include "rcl/publisher.h" -#include "rcl/service.h" -#include "rcl/time.h" -#include "rmw/rmw.h" - -#define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" - -typedef struct rcl_service_introspection_utils_s { - rcl_clock_t * clock; - rcl_publisher_t * publisher; - rosidl_message_type_support_t * request_type_support; - rosidl_message_type_support_t * response_type_support; - char * service_name; - char * service_type_name; -} rcl_service_introspection_utils_t; - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_service_introspection_utils_t -rcl_get_zero_initialized_introspection_utils(); - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_service_introspection_init( - rcl_service_introspection_utils_t * introspection_utils, - const rosidl_service_type_support_t * service_type_support, - const char * service_name, - const rcl_node_t * node, - rcl_allocator_t * allocator); - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_service_introspection_fini( - rcl_service_introspection_utils_t * introspection_utils, - rcl_allocator_t * allocator, - rcl_node_t * node); - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_introspection_send_message( - const rcl_service_introspection_utils_t * introspection_utils, - const uint8_t event_type, - const void * ros_response_request, - const int64_t sequence_number, - const uint8_t uuid[16], // uuid is uint8_t but the guid is int8_t - const rcl_allocator_t * allocator); - - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_service_introspection_disable(rcl_service_t * service); - - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_service_introspection_enable(rcl_service_t * service); - - - -#endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index d8e8685a5..8759452d2 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -37,6 +37,7 @@ extern "C" #include "tracetools/tracetools.h" +#include "rcl/introspection.h" #include "rosidl_runtime_c/service_type_support_struct.h" #include "rosidl_runtime_c/message_type_support_struct.h" @@ -141,7 +142,8 @@ rcl_service_init( RCL_SET_ERROR_MSG(rmw_get_error_string().str); goto fail; } - + + // enabled by default service->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( sizeof(rcl_service_introspection_utils_t), allocator->state); *service->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h index 0c418c26c..b1c8386c6 100644 --- a/rcl/src/rcl/service_impl.h +++ b/rcl/src/rcl/service_impl.h @@ -18,7 +18,8 @@ #include "rcl/service.h" -#include "./introspection.h" + +#include "rcl/introspection.h" struct rcl_service_impl_s { From d1a6e6146746fdb43a37f774b24370ddd7fe4623 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 7 Jul 2022 18:50:05 -0700 Subject: [PATCH 11/16] support enable/disable service introspection events and content Signed-off-by: Brian Chen --- rcl/include/rcl/introspection.h | 13 +- rcl/src/rcl/introspection.c | 264 +++++++++++++++++++++++++++----- 2 files changed, 237 insertions(+), 40 deletions(-) diff --git a/rcl/include/rcl/introspection.h b/rcl/include/rcl/introspection.h index 9060ffd62..29fc0ba28 100644 --- a/rcl/include/rcl/introspection.h +++ b/rcl/include/rcl/introspection.h @@ -142,11 +142,20 @@ rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_ */ RCL_PUBLIC void -rcl_service_introspection_configure_client_event_content(rcl_client_t * client, bool enable); +rcl_service_introspection_enable_client_content(rcl_client_t * client); RCL_PUBLIC void -rcl_service_introspection_configure_service_event_content(rcl_service_t * service, bool enable); +rcl_service_introspection_enable_service_content(rcl_service_t * service); + +RCL_PUBLIC +void +rcl_service_introspection_disable_client_content(rcl_client_t * client); + +RCL_PUBLIC +void +rcl_service_introspection_disable_service_content(rcl_service_t * service); + // TODO(ihasdapie): Do we want some getters for if content and/or introspection is enabled/disabled? diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index f2230dc0c..7227dab77 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -54,6 +54,9 @@ rcl_get_zero_initialized_introspection_utils() .clock = NULL, .service_name = NULL, .service_type_name = NULL, + .service_event_topic_name = NULL, + ._enabled = true, + ._content_enabled = true, }; return null_introspection_utils; } @@ -68,6 +71,7 @@ rcl_service_typesupport_to_message_typesupport( { rcutils_ret_t ret; type_support_map_t * map = (type_support_map_t *)service_typesupport->data; + // TODO(ihasdapie): change to #define or some constant const char * typesupport_library_fmt = "lib%s__rosidl_typesupport_c.so"; const char * service_message_fmt = // package_name, type name, Request/Response "rosidl_typesupport_c__get_message_type_support_handle__%s__srv__%s_%s"; @@ -146,11 +150,17 @@ rcl_ret_t rcl_service_introspection_init( &introspection_utils->response_type_support, allocator); // Make a publisher - char * service_event_name = (char *)allocator->zero_allocate( + char * service_event_topic_name = (char *)allocator->zero_allocate( strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, sizeof(char), allocator->state); - strcpy(service_event_name, service_name); - strcat(service_event_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + strcpy(service_event_topic_name, service_name); + strcat(service_event_topic_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + + introspection_utils->service_event_topic_name = allocator->allocate( + strlen(service_event_topic_name) + 1, allocator->state); + + strcpy(introspection_utils->service_event_topic_name, service_event_topic_name); + introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); @@ -164,7 +174,7 @@ rcl_ret_t rcl_service_introspection_init( rcl_publisher_get_default_options(); ret = rcl_publisher_init( introspection_utils->publisher, node, - service_event_typesupport, service_event_name, + service_event_typesupport, service_event_topic_name, &publisher_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); @@ -179,7 +189,7 @@ rcl_ret_t rcl_service_introspection_init( return RCL_RET_ERROR; } - allocator->deallocate(service_event_name, allocator->state); + allocator->deallocate(service_event_topic_name, allocator->state); return RCL_RET_OK; } @@ -187,7 +197,8 @@ rcl_ret_t rcl_service_introspection_init( rcl_ret_t rcl_service_introspection_fini( rcl_service_introspection_utils_t * introspection_utils, - rcl_allocator_t * allocator, rcl_node_t * node) + rcl_allocator_t * allocator, + rcl_node_t * node) { rcl_ret_t ret; ret = rcl_publisher_fini(introspection_utils->publisher, node); @@ -204,6 +215,14 @@ rcl_service_introspection_fini( allocator->deallocate(introspection_utils->publisher, allocator->state); allocator->deallocate(introspection_utils->clock, allocator->state); allocator->deallocate(introspection_utils->service_name, allocator->state); + allocator->deallocate(introspection_utils->service_event_topic_name, allocator->state); + allocator->deallocate(introspection_utils->service_type_name, allocator->state); + + introspection_utils->publisher = NULL; + introspection_utils->clock = NULL; + introspection_utils->service_name = NULL; + introspection_utils->service_event_topic_name = NULL; + introspection_utils->service_type_name = NULL; return RCL_RET_OK; } @@ -217,6 +236,10 @@ rcl_introspection_send_message( const uint8_t uuid[16], const rcl_allocator_t * allocator) { + // Early exit of service introspection if it isn't enabled + // TODO(ihasdapie): Different return code? + if (!introspection_utils->_enabled) return RCL_RET_OK; + rcl_ret_t ret; rcl_interfaces__msg__ServiceEvent msg; @@ -225,37 +248,40 @@ rcl_introspection_send_message( rcl_interfaces__msg__ServiceEventInfo msg_info; rcl_interfaces__msg__ServiceEventInfo__init(&msg_info); - // build serialized message rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); - ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); - rosidl_message_type_support_t * serialized_message_ts; - switch (event_type) { - case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: - serialized_message_ts = introspection_utils->request_type_support; - break; - case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: - serialized_message_ts = introspection_utils->request_type_support; - break; - case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: - serialized_message_ts = introspection_utils->response_type_support; - break; - case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED: - serialized_message_ts = introspection_utils->response_type_support; - break; - default: - RCL_SET_ERROR_MSG("Invalid event type"); + if (introspection_utils->_content_enabled) { + // build serialized message + ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); + rosidl_message_type_support_t * serialized_message_ts; + switch (event_type) { + case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: + serialized_message_ts = introspection_utils->request_type_support; + break; + case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: + serialized_message_ts = introspection_utils->request_type_support; + break; + case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: + serialized_message_ts = introspection_utils->response_type_support; + break; + case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED: + serialized_message_ts = introspection_utils->response_type_support; + break; + default: + RCL_SET_ERROR_MSG("Invalid event type"); + return RCL_RET_ERROR; + } + + + ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; + } + rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); + memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); } - ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); - memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); - // populate info message msg_info.event_type = event_type; msg_info.sequence_number = sequence_number; @@ -290,18 +316,180 @@ rcl_introspection_send_message( } // clean up - unique_identifier_msgs__msg__UUID__fini(&uuid_msg); - builtin_interfaces__msg__Time__fini(×tamp); - ret = rmw_serialized_message_fini(&serialized_message); - if (RMW_RET_OK != ret) { + if (introspection_utils->_content_enabled) { + ret = rmw_serialized_message_fini(&serialized_message); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + } + + rcl_interfaces__msg__ServiceEvent__fini(&msg); + return RCL_RET_OK; +} + + + +rcl_ret_t +rcl_service_introspection_enable( + rcl_service_introspection_utils_t * introspection_utils, + const rcl_node_t * node, + rcl_allocator_t * allocator) +{ + rcl_ret_t ret; + + introspection_utils->publisher = + allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + *introspection_utils->publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * service_event_typesupport = + ROSIDL_GET_MSG_TYPE_SUPPORT( + rcl_interfaces, msg, + ServiceEvent); + + rcl_publisher_options_t publisher_options = + rcl_publisher_get_default_options(); + ret = rcl_publisher_init( + introspection_utils->publisher, node, + service_event_typesupport, introspection_utils->service_event_topic_name, + &publisher_options); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + introspection_utils->clock = allocator->allocate(sizeof(rcl_clock_t), allocator->state); + ret = rcl_clock_init(RCL_STEADY_TIME, introspection_utils->clock, allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + introspection_utils->_enabled = true; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_service_introspection_disable( + rcl_service_introspection_utils_t * introspection_utils, + rcl_node_t * node, + const rcl_allocator_t * allocator) +{ + rcl_ret_t ret; + ret = rcl_publisher_fini(introspection_utils->publisher, node); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + ret = rcl_clock_fini(introspection_utils->clock); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + allocator->deallocate(introspection_utils->publisher, allocator->state); + allocator->deallocate(introspection_utils->clock, allocator->state); + introspection_utils->publisher = NULL; + introspection_utils->clock = NULL; + + introspection_utils->_enabled = false; + return RCL_RET_OK; +} + + +rcl_ret_t +rcl_service_introspection_enable_service_events(rcl_service_t * service, rcl_node_t * node){ + rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils; + rcl_ret_t ret = rcl_service_introspection_enable( + introspection_utils, node, &service->impl->options.allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + + +rcl_ret_t +rcl_service_introspection_disable_service_events(rcl_service_t * service, rcl_node_t * node){ + rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils; + rcl_ret_t ret = rcl_service_introspection_disable( + introspection_utils, node, &service->impl->options.allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_service_introspection_enable_client_events(rcl_client_t * client, rcl_node_t * node){ + rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils; + rcl_ret_t ret = rcl_service_introspection_enable( + introspection_utils, node, &client->impl->options.allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; + +} + +rcl_ret_t +rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_t * node){ + rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils; + rcl_ret_t ret = rcl_service_introspection_disable( + introspection_utils, node, &client->impl->options.allocator); + if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - // rcl_interfaces__msg__ServiceEventInfo__fini(&msg_info); - // rcl_interfaces__msg__ServiceEvent__fini(&msg); return RCL_RET_OK; } + +void +rcl_service_introspection_enable_client_content(rcl_client_t * client) +{ + client->impl->introspection_utils->_content_enabled = true; +} + +void +rcl_service_introspection_enable_service_content(rcl_service_t * service) +{ + service->impl->introspection_utils->_content_enabled = true; +} + +void +rcl_service_introspection_disable_client_content(rcl_client_t * client) +{ + client->impl->introspection_utils->_content_enabled = false; +} + +void +rcl_service_introspection_disable_service_content(rcl_service_t * service) +{ + service->impl->introspection_utils->_content_enabled = false; +} + + + #ifdef __cplusplus } #endif + + + + + + + + + + + + + + From fa4cd782ee1759bfaa5354faea711b5c273ec1da Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 8 Jul 2022 11:38:18 -0700 Subject: [PATCH 12/16] clang-format Signed-off-by: Brian Chen --- rcl/src/rcl/introspection.c | 240 ++++++++++++++---------------------- 1 file changed, 90 insertions(+), 150 deletions(-) diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 7227dab77..12fac4c94 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -13,39 +13,33 @@ // limitations under the License. #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif #include "rcl/introspection.h" -#include "./service_impl.h" -#include "./client_impl.h" - -#include "rcl/error_handling.h" -#include "rmw/error_handling.h" +#include +#include "./client_impl.h" +#include "./service_impl.h" +#include "builtin_interfaces/msg/time.h" #include "dlfcn.h" - +#include "rcl/error_handling.h" +#include "rcl_interfaces/msg/service_event.h" +#include "rcl_interfaces/msg/service_event_info.h" +#include "rcl_interfaces/msg/service_event_type.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rcutils/shared_library.h" +#include "rmw/error_handling.h" #include "rosidl_runtime_c/primitives_sequence_functions.h" #include "rosidl_runtime_c/string_functions.h" - -#include "builtin_interfaces/msg/time.h" -#include "rcl_interfaces/msg/service_event.h" -#include "rcl_interfaces/msg/service_event_type.h" -#include "rcl_interfaces/msg/service_event_info.h" -#include "unique_identifier_msgs/msg/uuid.h" - -#include "rosidl_typesupport_introspection_c/service_introspection.h" #include "rosidl_typesupport_c/type_support_map.h" #include "rosidl_typesupport_introspection_c/identifier.h" -#include +#include "rosidl_typesupport_introspection_c/service_introspection.h" +#include "unique_identifier_msgs/msg/uuid.h" -rcl_service_introspection_utils_t -rcl_get_zero_initialized_introspection_utils() +rcl_service_introspection_utils_t rcl_get_zero_initialized_introspection_utils() { static rcl_service_introspection_utils_t null_introspection_utils = { .response_type_support = NULL, @@ -61,13 +55,10 @@ rcl_get_zero_initialized_introspection_utils() return null_introspection_utils; } - -rcl_ret_t -rcl_service_typesupport_to_message_typesupport( +rcl_ret_t rcl_service_typesupport_to_message_typesupport( const rosidl_service_type_support_t * service_typesupport, rosidl_message_type_support_t ** request_typesupport, - rosidl_message_type_support_t ** response_typesupport, - const rcl_allocator_t * allocator) + rosidl_message_type_support_t ** response_typesupport, const rcl_allocator_t * allocator) { rcutils_ret_t ret; type_support_map_t * map = (type_support_map_t *)service_typesupport->data; @@ -77,28 +68,25 @@ rcl_service_typesupport_to_message_typesupport( "rosidl_typesupport_c__get_message_type_support_handle__%s__srv__%s_%s"; const char * service_type_name = rcl_service_get_service_type_name(service_typesupport); - + // build out typesupport library and symbol names char * typesupport_library_name = allocator->allocate( - sizeof(char) * ((strlen(typesupport_library_fmt) - 2) + - strlen(map->package_name) + 1), + sizeof(char) * ((strlen(typesupport_library_fmt) - 2) + strlen(map->package_name) + 1), allocator->state); char * request_message_symbol = allocator->allocate( - sizeof(char) * - ((strlen(service_message_fmt) - 6) + strlen(map->package_name) + - strlen(service_type_name) + strlen("Request") + 1), + sizeof(char) * ((strlen(service_message_fmt) - 6) + strlen(map->package_name) + + strlen(service_type_name) + strlen("Request") + 1), allocator->state); char * response_message_symbol = allocator->allocate( - sizeof(char) * - ((strlen(service_message_fmt) - 6) + strlen(map->package_name) + - strlen(service_type_name) + strlen("Request") + 1), + sizeof(char) * ((strlen(service_message_fmt) - 6) + strlen(map->package_name) + + strlen(service_type_name) + strlen("Request") + 1), allocator->state); sprintf(typesupport_library_name, typesupport_library_fmt, map->package_name); - sprintf(request_message_symbol, service_message_fmt, map->package_name, - service_type_name, "Request"); - sprintf(response_message_symbol, service_message_fmt, map->package_name, - service_type_name, "Response"); + sprintf( + request_message_symbol, service_message_fmt, map->package_name, service_type_name, "Request"); + sprintf( + response_message_symbol, service_message_fmt, map->package_name, service_type_name, "Response"); rcutils_shared_library_t typesupport_library = rcutils_get_zero_initialized_shared_library(); ret = rcutils_load_shared_library(&typesupport_library, typesupport_library_name, *allocator); @@ -106,17 +94,17 @@ rcl_service_typesupport_to_message_typesupport( return RCL_RET_ERROR; } - rosidl_message_type_support_t *(*req_typesupport_func_handle)() = + rosidl_message_type_support_t * (*req_typesupport_func_handle)() = (rosidl_message_type_support_t * (*)()) - rcutils_get_symbol(&typesupport_library, request_message_symbol); - RCL_CHECK_FOR_NULL_WITH_MSG(req_typesupport_func_handle, - "Looking up request type support failed", return RCL_RET_ERROR); + rcutils_get_symbol(&typesupport_library, request_message_symbol); + RCL_CHECK_FOR_NULL_WITH_MSG( + req_typesupport_func_handle, "Looking up request type support failed", return RCL_RET_ERROR); - rosidl_message_type_support_t *(*resp_typesupport_func_handle)() = + rosidl_message_type_support_t * (*resp_typesupport_func_handle)() = (rosidl_message_type_support_t * (*)()) - rcutils_get_symbol(&typesupport_library, response_message_symbol); - RCL_CHECK_FOR_NULL_WITH_MSG(resp_typesupport_func_handle, - "Looking up response type support failed", return RCL_RET_ERROR); + rcutils_get_symbol(&typesupport_library, response_message_symbol); + RCL_CHECK_FOR_NULL_WITH_MSG( + resp_typesupport_func_handle, "Looking up response type support failed", return RCL_RET_ERROR); *request_typesupport = req_typesupport_func_handle(); *response_typesupport = resp_typesupport_func_handle(); @@ -129,52 +117,44 @@ rcl_service_typesupport_to_message_typesupport( rcl_ret_t rcl_service_introspection_init( rcl_service_introspection_utils_t * introspection_utils, - const rosidl_service_type_support_t * service_type_support, - const char * service_name, - const rcl_node_t * node, - rcl_allocator_t * allocator) + const rosidl_service_type_support_t * service_type_support, const char * service_name, + const rcl_node_t * node, rcl_allocator_t * allocator) { rcl_ret_t ret; - introspection_utils->service_name = allocator->allocate( - strlen(service_name) + 1, allocator->state); + introspection_utils->service_name = + allocator->allocate(strlen(service_name) + 1, allocator->state); strcpy(introspection_utils->service_name, service_name); - const char* service_type_name = rcl_service_get_service_type_name(service_type_support); - introspection_utils->service_type_name = allocator->allocate( - strlen(service_type_name) + 1, allocator->state); + const char * service_type_name = rcl_service_get_service_type_name(service_type_support); + introspection_utils->service_type_name = + allocator->allocate(strlen(service_type_name) + 1, allocator->state); strcpy(introspection_utils->service_type_name, service_type_name); rcl_service_typesupport_to_message_typesupport( - service_type_support, &introspection_utils->request_type_support, + service_type_support, &introspection_utils->request_type_support, &introspection_utils->response_type_support, allocator); // Make a publisher char * service_event_topic_name = (char *)allocator->zero_allocate( - strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, - sizeof(char), allocator->state); + strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, sizeof(char), + allocator->state); strcpy(service_event_topic_name, service_name); strcat(service_event_topic_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); - introspection_utils->service_event_topic_name = allocator->allocate( - strlen(service_event_topic_name) + 1, allocator->state); + introspection_utils->service_event_topic_name = + allocator->allocate(strlen(service_event_topic_name) + 1, allocator->state); strcpy(introspection_utils->service_event_topic_name, service_event_topic_name); - - introspection_utils->publisher = - allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); *introspection_utils->publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * service_event_typesupport = - ROSIDL_GET_MSG_TYPE_SUPPORT( - rcl_interfaces, msg, - ServiceEvent); + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); - rcl_publisher_options_t publisher_options = - rcl_publisher_get_default_options(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init( - introspection_utils->publisher, node, - service_event_typesupport, service_event_topic_name, + introspection_utils->publisher, node, service_event_typesupport, service_event_topic_name, &publisher_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); @@ -194,10 +174,8 @@ rcl_ret_t rcl_service_introspection_init( return RCL_RET_OK; } -rcl_ret_t -rcl_service_introspection_fini( - rcl_service_introspection_utils_t * introspection_utils, - rcl_allocator_t * allocator, +rcl_ret_t rcl_service_introspection_fini( + rcl_service_introspection_utils_t * introspection_utils, rcl_allocator_t * allocator, rcl_node_t * node) { rcl_ret_t ret; @@ -227,13 +205,9 @@ rcl_service_introspection_fini( return RCL_RET_OK; } -rcl_ret_t -rcl_introspection_send_message( - const rcl_service_introspection_utils_t * introspection_utils, - const uint8_t event_type, - const void * ros_response_request, - const int64_t sequence_number, - const uint8_t uuid[16], +rcl_ret_t rcl_introspection_send_message( + const rcl_service_introspection_utils_t * introspection_utils, const uint8_t event_type, + const void * ros_response_request, const int64_t sequence_number, const uint8_t uuid[16], const rcl_allocator_t * allocator) { // Early exit of service introspection if it isn't enabled @@ -241,7 +215,7 @@ rcl_introspection_send_message( if (!introspection_utils->_enabled) return RCL_RET_OK; rcl_ret_t ret; - + rcl_interfaces__msg__ServiceEvent msg; rcl_interfaces__msg__ServiceEvent__init(&msg); @@ -271,17 +245,16 @@ rcl_introspection_send_message( return RCL_RET_ERROR; } - ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); if (RMW_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } - rosidl_runtime_c__octet__Sequence__init(&msg.serialized_event, serialized_message.buffer_length); + rosidl_runtime_c__octet__Sequence__init( + &msg.serialized_event, serialized_message.buffer_length); memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); } - // populate info message msg_info.event_type = event_type; msg_info.sequence_number = sequence_number; @@ -328,30 +301,21 @@ rcl_introspection_send_message( return RCL_RET_OK; } - - -rcl_ret_t -rcl_service_introspection_enable( - rcl_service_introspection_utils_t * introspection_utils, - const rcl_node_t * node, - rcl_allocator_t * allocator) +rcl_ret_t rcl_service_introspection_enable( + rcl_service_introspection_utils_t * introspection_utils, const rcl_node_t * node, + rcl_allocator_t * allocator) { rcl_ret_t ret; - introspection_utils->publisher = - allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); *introspection_utils->publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * service_event_typesupport = - ROSIDL_GET_MSG_TYPE_SUPPORT( - rcl_interfaces, msg, - ServiceEvent); + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent); - rcl_publisher_options_t publisher_options = - rcl_publisher_get_default_options(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init( - introspection_utils->publisher, node, - service_event_typesupport, introspection_utils->service_event_topic_name, - &publisher_options); + introspection_utils->publisher, node, service_event_typesupport, + introspection_utils->service_event_topic_name, &publisher_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); return RCL_RET_ERROR; @@ -368,11 +332,9 @@ rcl_service_introspection_enable( return RCL_RET_OK; } -rcl_ret_t -rcl_service_introspection_disable( - rcl_service_introspection_utils_t * introspection_utils, - rcl_node_t * node, - const rcl_allocator_t * allocator) +rcl_ret_t rcl_service_introspection_disable( + rcl_service_introspection_utils_t * introspection_utils, rcl_node_t * node, + const rcl_allocator_t * allocator) { rcl_ret_t ret; ret = rcl_publisher_fini(introspection_utils->publisher, node); @@ -396,12 +358,12 @@ rcl_service_introspection_disable( return RCL_RET_OK; } - -rcl_ret_t -rcl_service_introspection_enable_service_events(rcl_service_t * service, rcl_node_t * node){ +rcl_ret_t rcl_service_introspection_enable_service_events( + rcl_service_t * service, rcl_node_t * node) +{ rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils; - rcl_ret_t ret = rcl_service_introspection_enable( - introspection_utils, node, &service->impl->options.allocator); + rcl_ret_t ret = + rcl_service_introspection_enable(introspection_utils, node, &service->impl->options.allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -410,12 +372,12 @@ rcl_service_introspection_enable_service_events(rcl_service_t * service, rcl_nod return RCL_RET_OK; } - -rcl_ret_t -rcl_service_introspection_disable_service_events(rcl_service_t * service, rcl_node_t * node){ +rcl_ret_t rcl_service_introspection_disable_service_events( + rcl_service_t * service, rcl_node_t * node) +{ rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils; - rcl_ret_t ret = rcl_service_introspection_disable( - introspection_utils, node, &service->impl->options.allocator); + rcl_ret_t ret = + rcl_service_introspection_disable(introspection_utils, node, &service->impl->options.allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -424,24 +386,23 @@ rcl_service_introspection_disable_service_events(rcl_service_t * service, rcl_no return RCL_RET_OK; } -rcl_ret_t -rcl_service_introspection_enable_client_events(rcl_client_t * client, rcl_node_t * node){ +rcl_ret_t rcl_service_introspection_enable_client_events(rcl_client_t * client, rcl_node_t * node) +{ rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils; - rcl_ret_t ret = rcl_service_introspection_enable( - introspection_utils, node, &client->impl->options.allocator); + rcl_ret_t ret = + rcl_service_introspection_enable(introspection_utils, node, &client->impl->options.allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } return RCL_RET_OK; - } -rcl_ret_t -rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_t * node){ +rcl_ret_t rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_t * node) +{ rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils; - rcl_ret_t ret = rcl_service_introspection_disable( - introspection_utils, node, &client->impl->options.allocator); + rcl_ret_t ret = + rcl_service_introspection_disable(introspection_utils, node, &client->impl->options.allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -449,47 +410,26 @@ rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_ return RCL_RET_OK; } - -void -rcl_service_introspection_enable_client_content(rcl_client_t * client) +void rcl_service_introspection_enable_client_content(rcl_client_t * client) { client->impl->introspection_utils->_content_enabled = true; } -void -rcl_service_introspection_enable_service_content(rcl_service_t * service) +void rcl_service_introspection_enable_service_content(rcl_service_t * service) { service->impl->introspection_utils->_content_enabled = true; } -void -rcl_service_introspection_disable_client_content(rcl_client_t * client) +void rcl_service_introspection_disable_client_content(rcl_client_t * client) { client->impl->introspection_utils->_content_enabled = false; } -void -rcl_service_introspection_disable_service_content(rcl_service_t * service) +void rcl_service_introspection_disable_service_content(rcl_service_t * service) { service->impl->introspection_utils->_content_enabled = false; } - - #ifdef __cplusplus } #endif - - - - - - - - - - - - - - From 2070595015675993aa389e4e3d205aed053f7cf1 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 8 Jul 2022 12:01:13 -0700 Subject: [PATCH 13/16] use case statement fallthrough Signed-off-by: Brian Chen --- rcl/src/rcl/introspection.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 12fac4c94..de5ef0892 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -225,18 +225,14 @@ rcl_ret_t rcl_introspection_send_message( rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); if (introspection_utils->_content_enabled) { // build serialized message - ret = rmw_serialized_message_init(&serialized_message, 0u, allocator); + ret = rmw_serialized_message_init(&serialized_message, 0U, allocator); rosidl_message_type_support_t * serialized_message_ts; switch (event_type) { case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT: - serialized_message_ts = introspection_utils->request_type_support; - break; case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED: serialized_message_ts = introspection_utils->request_type_support; break; case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT: - serialized_message_ts = introspection_utils->response_type_support; - break; case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED: serialized_message_ts = introspection_utils->response_type_support; break; From 5a1a990730b80682cc822c886ce14a1fc6419c32 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 8 Jul 2022 14:59:15 -0700 Subject: [PATCH 14/16] consolidate service introspection enable/disable to service introspection configure Signed-off-by: Brian Chen --- rcl/include/rcl/introspection.h | 33 +++++----------- rcl/src/rcl/introspection.c | 70 +++++++++++---------------------- 2 files changed, 34 insertions(+), 69 deletions(-) diff --git a/rcl/include/rcl/introspection.h b/rcl/include/rcl/introspection.h index 29fc0ba28..28d765734 100644 --- a/rcl/include/rcl/introspection.h +++ b/rcl/include/rcl/introspection.h @@ -119,22 +119,17 @@ rcl_service_introspection_disable( RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_service_introspection_enable_service_events(rcl_service_t * service, rcl_node_t * node); - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_service_introspection_disable_service_events(rcl_service_t * service, rcl_node_t * node); - -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_service_introspection_enable_client_events(rcl_client_t * client, rcl_node_t * node); +rcl_service_introspection_configure_service_events( + rcl_service_t * service, + rcl_node_t * node, + bool enable); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_t * node); +rcl_service_introspection_configure_client_events(rcl_client_t * client, + rcl_node_t * node, + bool enable); /* @@ -142,23 +137,15 @@ rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_ */ RCL_PUBLIC void -rcl_service_introspection_enable_client_content(rcl_client_t * client); - -RCL_PUBLIC -void -rcl_service_introspection_enable_service_content(rcl_service_t * service); - -RCL_PUBLIC -void -rcl_service_introspection_disable_client_content(rcl_client_t * client); +rcl_service_introspection_configure_client_content(rcl_client_t * client, bool enable); RCL_PUBLIC void -rcl_service_introspection_disable_service_content(rcl_service_t * service); +rcl_service_introspection_configure_service_content(rcl_service_t * service, bool enable); -// TODO(ihasdapie): Do we want some getters for if content and/or introspection is enabled/disabled? +// TODO(ihasdapie): Do we want some getters for if content and/or introspection enabled/disabled? diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index de5ef0892..092cb46a7 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -354,39 +354,19 @@ rcl_ret_t rcl_service_introspection_disable( return RCL_RET_OK; } -rcl_ret_t rcl_service_introspection_enable_service_events( - rcl_service_t * service, rcl_node_t * node) +rcl_ret_t rcl_service_introspection_configure_service_events( + rcl_service_t * service, rcl_node_t * node, bool enable) { rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils; - rcl_ret_t ret = - rcl_service_introspection_enable(introspection_utils, node, &service->impl->options.allocator); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - - return RCL_RET_OK; -} + rcl_ret_t ret; -rcl_ret_t rcl_service_introspection_disable_service_events( - rcl_service_t * service, rcl_node_t * node) -{ - rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils; - rcl_ret_t ret = - rcl_service_introspection_disable(introspection_utils, node, &service->impl->options.allocator); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; + if (enable) { + ret = rcl_service_introspection_enable( + introspection_utils, node, &service->impl->options.allocator); + } else { + ret = rcl_service_introspection_disable( + introspection_utils, node, &service->impl->options.allocator); } - - return RCL_RET_OK; -} - -rcl_ret_t rcl_service_introspection_enable_client_events(rcl_client_t * client, rcl_node_t * node) -{ - rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils; - rcl_ret_t ret = - rcl_service_introspection_enable(introspection_utils, node, &client->impl->options.allocator); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -394,11 +374,19 @@ rcl_ret_t rcl_service_introspection_enable_client_events(rcl_client_t * client, return RCL_RET_OK; } -rcl_ret_t rcl_service_introspection_disable_client_events(rcl_client_t * client, rcl_node_t * node) +rcl_ret_t rcl_service_introspection_configure_client_events( + rcl_client_t * client, rcl_node_t * node, bool enable) { rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils; - rcl_ret_t ret = - rcl_service_introspection_disable(introspection_utils, node, &client->impl->options.allocator); + rcl_ret_t ret; + + if (enable) { + ret = + rcl_service_introspection_enable(introspection_utils, node, &client->impl->options.allocator); + } else { + ret = rcl_service_introspection_disable( + introspection_utils, node, &client->impl->options.allocator); + } if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -406,24 +394,14 @@ rcl_ret_t rcl_service_introspection_disable_client_events(rcl_client_t * client, return RCL_RET_OK; } -void rcl_service_introspection_enable_client_content(rcl_client_t * client) -{ - client->impl->introspection_utils->_content_enabled = true; -} - -void rcl_service_introspection_enable_service_content(rcl_service_t * service) -{ - service->impl->introspection_utils->_content_enabled = true; -} - -void rcl_service_introspection_disable_client_content(rcl_client_t * client) +void rcl_service_introspection_configure_client_content(rcl_client_t * client, bool enable) { - client->impl->introspection_utils->_content_enabled = false; + client->impl->introspection_utils->_content_enabled = enable; } -void rcl_service_introspection_disable_service_content(rcl_service_t * service) +void rcl_service_introspection_configure_service_content(rcl_service_t * service, bool enable) { - service->impl->introspection_utils->_content_enabled = false; + service->impl->introspection_utils->_content_enabled = enable; } #ifdef __cplusplus From 78f85f0ea133f90983fa4f3788b5f2ed56cd6780 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 13 Jul 2022 18:17:37 -0700 Subject: [PATCH 15/16] remove rosidl_typesupport_introspection_c dependency Signed-off-by: Brian Chen --- rcl/CMakeLists.txt | 1 - rcl/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 4293a1730..6b48615dd 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -126,7 +126,6 @@ ament_export_dependencies(rcutils) ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(tracetools) -ament_export_dependencies(rosidl_typesupport_introspection_c) ament_export_dependencies(rosidl_typesupport_c) if(BUILD_TESTING) diff --git a/rcl/package.xml b/rcl/package.xml index 18da61d6e..6679c1f25 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -24,7 +24,6 @@ rosidl_runtime_c tracetools - rosidl_typesupport_introspection_c rosidl_typesupport_c ament_cmake_gtest From 0933ee668139ec05c5eb4e5fb543bd582880e1d0 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 13 Jul 2022 18:19:06 -0700 Subject: [PATCH 16/16] request clock from user for service introspection timestamps Signed-off-by: Brian Chen --- rcl/include/rcl/client.h | 5 ++ rcl/include/rcl/introspection.h | 16 ++++-- rcl/include/rcl/service.h | 8 +++ rcl/src/rcl/client.c | 74 +++++++++++++++---------- rcl/src/rcl/introspection.c | 42 +++----------- rcl/src/rcl/service.c | 98 +++++++++++++++++++-------------- rcl/src/rcl/service_impl.h | 1 - 7 files changed, 135 insertions(+), 109 deletions(-) diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index 224f74b8c..f7d28651d 100644 --- a/rcl/include/rcl/client.h +++ b/rcl/include/rcl/client.h @@ -27,6 +27,7 @@ extern "C" #include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" +#include "rcl/time.h" #include "rcl/visibility_control.h" /// Internal rcl client implementation struct. @@ -47,6 +48,10 @@ typedef struct rcl_client_options_s /// Custom allocator for the client, used for incidental allocations. /** For default behavior (malloc/free), use: rcl_get_default_allocator() */ rcl_allocator_t allocator; + /// Enable/Disable service introspection features + bool enable_service_introspection; + /// The clock to use for service introspection message timestampes + rcl_clock_t * clock; } rcl_client_options_t; /// Return a rcl_client_t struct with members set to `NULL`. diff --git a/rcl/include/rcl/introspection.h b/rcl/include/rcl/introspection.h index 28d765734..58cf8b0a1 100644 --- a/rcl/include/rcl/introspection.h +++ b/rcl/include/rcl/introspection.h @@ -22,9 +22,10 @@ extern "C" #include "rcl/publisher.h" #include "rcl/service.h" +#include "rcl/client.h" #include "rcl/time.h" #include "rmw/rmw.h" -#include "rcl/client.h" +#include "stdbool.h" #define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" @@ -36,10 +37,15 @@ typedef struct rcl_service_introspection_utils_s { char * service_name; char * service_type_name; char * service_event_topic_name; + + + // enable/disable service introspection during runtime bool _enabled; + // enable/disable passing along service introspection content during runtime bool _content_enabled; } rcl_service_introspection_utils_t; + RCL_PUBLIC RCL_WARN_UNUSED rcl_service_introspection_utils_t @@ -53,6 +59,7 @@ rcl_service_introspection_init( const rosidl_service_type_support_t * service_type_support, const char * service_name, const rcl_node_t * node, + const rcl_clock_t * clock, rcl_allocator_t * allocator); RCL_PUBLIC @@ -68,9 +75,9 @@ RCL_WARN_UNUSED rcl_ret_t rcl_introspection_send_message( const rcl_service_introspection_utils_t * introspection_utils, - const uint8_t event_type, + uint8_t event_type, const void * ros_response_request, - const int64_t sequence_number, + int64_t sequence_number, const uint8_t uuid[16], // uuid is uint8_t but the guid is int8_t const rcl_allocator_t * allocator); @@ -127,7 +134,8 @@ rcl_service_introspection_configure_service_events( RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_service_introspection_configure_client_events(rcl_client_t * client, +rcl_service_introspection_configure_client_events( + rcl_client_t * client, rcl_node_t * node, bool enable); diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index f30187358..0fad89ee9 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -17,6 +17,7 @@ #ifndef RCL__SERVICE_H_ #define RCL__SERVICE_H_ +#include "rcl/time.h" #ifdef __cplusplus extern "C" { @@ -28,6 +29,7 @@ extern "C" #include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" +// #include "rcl/introspection.h" // TODO(ihasdapie): Dependency cycle. Instead let's just put the clock into service_options. /// Internal rcl implementation struct. typedef struct rcl_service_impl_s rcl_service_impl_t; @@ -47,6 +49,11 @@ typedef struct rcl_service_options_s /// Custom allocator for the service, used for incidental allocations. /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ rcl_allocator_t allocator; + /// Enable/Disable service introspection features + bool enable_service_introspection; + + /// The clock to use for service introspection message timestampes + rcl_clock_t * clock; } rcl_service_options_t; /// Return a rcl_service_t struct with members set to `NULL`. @@ -198,6 +205,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node); * * - qos = rmw_qos_profile_services_default * - allocator = rcl_get_default_allocator() + * - */ RCL_PUBLIC RCL_WARN_UNUSED diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 70df329b4..9a1c3828a 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -108,14 +108,17 @@ rcl_client_init( RCL_SET_ERROR_MSG(rmw_get_error_string().str); goto fail; } + + client->impl->introspection_utils = NULL; + if (options->enable_service_introspection) { + client->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( + sizeof(rcl_service_introspection_utils_t), allocator->state); + *client->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); + ret = rcl_service_introspection_init( + client->impl->introspection_utils, type_support, + remapped_service_name, node, options->clock, allocator); + } - client->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( - sizeof(rcl_service_introspection_utils_t), allocator->state); - - *client->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); - ret = rcl_service_introspection_init( - client->impl->introspection_utils, type_support, - remapped_service_name, node, allocator); // get actual qos, and store it rmw_ret_t rmw_ret = rmw_client_request_publisher_get_actual_qos( @@ -156,6 +159,10 @@ rcl_client_init( remapped_service_name); goto cleanup; fail: + if (client->impl->introspection_utils) { + allocator->deallocate(client->impl->introspection_utils, allocator->state); + client->impl->introspection_utils = NULL; + } if (client->impl) { allocator->deallocate(client->impl, allocator->state); client->impl = NULL; @@ -206,6 +213,8 @@ rcl_client_get_default_options() // Must set the allocator and qos after because they are not a compile time constant. default_options.qos = rmw_qos_profile_services_default; default_options.allocator = rcl_get_default_allocator(); + default_options.clock = NULL; + default_options.enable_service_introspection = false; return default_options; } @@ -256,18 +265,22 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t } rcutils_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number); - rcl_ret_t rclret = rcl_introspection_send_message( - client->impl->introspection_utils, - rcl_interfaces__msg__ServiceEventType__REQUEST_SENT, - ros_request, - *sequence_number, - client->impl->rmw_handle->writer_guid, - &client->impl->options.allocator); - - if (RCL_RET_OK != rclret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; + + if (rcl_client_get_options(client)->enable_service_introspection) { + rcl_ret_t ret = rcl_introspection_send_message( + client->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__REQUEST_SENT, + ros_request, + *sequence_number, + client->impl->rmw_handle->writer_guid, + &client->impl->options.allocator); + + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } } + return RCL_RET_OK; } @@ -300,18 +313,21 @@ rcl_take_response_with_info( return RCL_RET_CLIENT_TAKE_FAILED; } - rcl_ret_t rclret = rcl_introspection_send_message( - client->impl->introspection_utils, - rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED, - ros_response, - request_header->request_id.sequence_number, - client->impl->rmw_handle->writer_guid, - &client->impl->options.allocator); - - if (RCL_RET_OK != rclret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; + if (rcl_client_get_options(client)->enable_service_introspection) { + rcl_ret_t ret = rcl_introspection_send_message( + client->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED, + ros_response, + request_header->request_id.sequence_number, + client->impl->rmw_handle->writer_guid, + &client->impl->options.allocator); + + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } } + return RCL_RET_OK; } diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 092cb46a7..fa98029fd 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -35,8 +35,6 @@ extern "C" { #include "rosidl_runtime_c/primitives_sequence_functions.h" #include "rosidl_runtime_c/string_functions.h" #include "rosidl_typesupport_c/type_support_map.h" -#include "rosidl_typesupport_introspection_c/identifier.h" -#include "rosidl_typesupport_introspection_c/service_introspection.h" #include "unique_identifier_msgs/msg/uuid.h" rcl_service_introspection_utils_t rcl_get_zero_initialized_introspection_utils() @@ -58,7 +56,8 @@ rcl_service_introspection_utils_t rcl_get_zero_initialized_introspection_utils() rcl_ret_t rcl_service_typesupport_to_message_typesupport( const rosidl_service_type_support_t * service_typesupport, rosidl_message_type_support_t ** request_typesupport, - rosidl_message_type_support_t ** response_typesupport, const rcl_allocator_t * allocator) + rosidl_message_type_support_t ** response_typesupport, + const rcl_allocator_t * allocator) { rcutils_ret_t ret; type_support_map_t * map = (type_support_map_t *)service_typesupport->data; @@ -117,8 +116,11 @@ rcl_ret_t rcl_service_typesupport_to_message_typesupport( rcl_ret_t rcl_service_introspection_init( rcl_service_introspection_utils_t * introspection_utils, - const rosidl_service_type_support_t * service_type_support, const char * service_name, - const rcl_node_t * node, rcl_allocator_t * allocator) + const rosidl_service_type_support_t * service_type_support, + const char * service_name, + const rcl_node_t * node, + const rcl_clock_t * clock, + rcl_allocator_t * allocator) { rcl_ret_t ret; @@ -162,13 +164,7 @@ rcl_ret_t rcl_service_introspection_init( } // make a clock - introspection_utils->clock = allocator->allocate(sizeof(rcl_clock_t), allocator->state); - ret = rcl_clock_init(RCL_STEADY_TIME, introspection_utils->clock, allocator); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; - } - + introspection_utils->clock = clock; allocator->deallocate(service_event_topic_name, allocator->state); return RCL_RET_OK; @@ -184,20 +180,13 @@ rcl_ret_t rcl_service_introspection_fini( RCL_SET_ERROR_MSG(rcl_get_error_string().str); return RCL_RET_ERROR; } - ret = rcl_clock_fini(introspection_utils->clock); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; - } allocator->deallocate(introspection_utils->publisher, allocator->state); - allocator->deallocate(introspection_utils->clock, allocator->state); allocator->deallocate(introspection_utils->service_name, allocator->state); allocator->deallocate(introspection_utils->service_event_topic_name, allocator->state); allocator->deallocate(introspection_utils->service_type_name, allocator->state); introspection_utils->publisher = NULL; - introspection_utils->clock = NULL; introspection_utils->service_name = NULL; introspection_utils->service_event_topic_name = NULL; introspection_utils->service_type_name = NULL; @@ -317,13 +306,6 @@ rcl_ret_t rcl_service_introspection_enable( return RCL_RET_ERROR; } - introspection_utils->clock = allocator->allocate(sizeof(rcl_clock_t), allocator->state); - ret = rcl_clock_init(RCL_STEADY_TIME, introspection_utils->clock, allocator); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; - } - introspection_utils->_enabled = true; return RCL_RET_OK; } @@ -339,16 +321,8 @@ rcl_ret_t rcl_service_introspection_disable( return RCL_RET_ERROR; } - ret = rcl_clock_fini(introspection_utils->clock); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - allocator->deallocate(introspection_utils->publisher, allocator->state); - allocator->deallocate(introspection_utils->clock, allocator->state); introspection_utils->publisher = NULL; - introspection_utils->clock = NULL; introspection_utils->_enabled = false; return RCL_RET_OK; diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 8759452d2..752bf9e98 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -27,7 +27,6 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" #include "rcl/graph.h" -#include "rcl/publisher.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -64,7 +63,8 @@ rcl_service_init( const rcl_node_t * node, const rosidl_service_type_support_t * type_support, const char * service_name, - const rcl_service_options_t * options) + const rcl_service_options_t * options + ) // user provide clock { RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); @@ -142,18 +142,22 @@ rcl_service_init( RCL_SET_ERROR_MSG(rmw_get_error_string().str); goto fail; } + + service->impl->introspection_utils = NULL; + if (options->enable_service_introspection) { + service->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( + sizeof(rcl_service_introspection_utils_t), allocator->state); + *service->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); + ret = rcl_service_introspection_init( + service->impl->introspection_utils, + type_support, + remapped_service_name, + node, + options->clock, + allocator + ); + } - // enabled by default - service->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate( - sizeof(rcl_service_introspection_utils_t), allocator->state); - *service->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils(); - ret = rcl_service_introspection_init( - service->impl->introspection_utils, - type_support, - remapped_service_name, - node, - allocator - ); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); @@ -198,9 +202,11 @@ rcl_service_init( goto cleanup; fail: if (service->impl) { - allocator->deallocate(service->impl->introspection_utils, allocator->state); + if (service->impl->introspection_utils) { + allocator->deallocate(service->impl->introspection_utils, allocator->state); + service->impl->introspection_utils = NULL; + } allocator->deallocate(service->impl, allocator->state); - service->impl->introspection_utils = NULL; service->impl = NULL; } ret = fail_ret; @@ -236,16 +242,19 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - ret = rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - result = RCL_RET_ERROR; + + if (service->impl->introspection_utils) { + ret = rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(service->impl->introspection_utils, allocator.state); + service->impl->introspection_utils = NULL; } - allocator.deallocate(service->impl->introspection_utils, allocator.state); allocator.deallocate(service->impl, allocator.state); service->impl = NULL; - if (ret != RCL_RET_OK) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); result = RCL_RET_ERROR; @@ -264,6 +273,8 @@ rcl_service_get_default_options() // Must set the allocator and qos after because they are not a compile time constant. default_options.qos = rmw_qos_profile_services_default; default_options.allocator = rcl_get_default_allocator(); + default_options.clock = NULL; + default_options.enable_service_introspection = false; return default_options; } @@ -342,16 +353,18 @@ rcl_take_request_with_info( return RCL_RET_SERVICE_TAKE_FAILED; } - rcl_ret_t rclret = rcl_introspection_send_message( - service->impl->introspection_utils, - rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED, - ros_request, - request_header->request_id.sequence_number, - request_header->request_id.writer_guid, - &options->allocator); - if (RCL_RET_OK != rclret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; + if (rcl_service_get_options(service)->enable_service_introspection) { + rcl_ret_t rclret = rcl_introspection_send_message( + service->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED, + ros_request, + request_header->request_id.sequence_number, + request_header->request_id.writer_guid, + &options->allocator); + if (RCL_RET_OK != rclret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } } return RCL_RET_OK; } @@ -394,16 +407,19 @@ rcl_send_response( } // publish out the introspected content - rcl_ret_t ret = rcl_introspection_send_message( - service->impl->introspection_utils, - rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, - ros_response, - request_header->sequence_number, - request_header->writer_guid, - &options->allocator); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG(rcl_get_error_string().str); - return RCL_RET_ERROR; + + if (rcl_service_get_options(service)->enable_service_introspection) { + rcl_ret_t ret = rcl_introspection_send_message( + service->impl->introspection_utils, + rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, + ros_response, + request_header->sequence_number, + request_header->writer_guid, + &options->allocator); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return RCL_RET_ERROR; + } } return RCL_RET_OK; } diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h index b1c8386c6..e27923a3e 100644 --- a/rcl/src/rcl/service_impl.h +++ b/rcl/src/rcl/service_impl.h @@ -18,7 +18,6 @@ #include "rcl/service.h" - #include "rcl/introspection.h" struct rcl_service_impl_s