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;
}