Skip to content

Commit

Permalink
Big simplification of the service_event_publisher system.
Browse files Browse the repository at this point in the history 
There was a lot of unnecessary redirection in there,
which turned out to be largely unnecessary since this
is an implementation detail.  Remove a lot of that
complexity, which removes a bunch of code.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
  • Loading branch information
@clalancette
clalancette committed Feb 21, 2023
1 parent 2f1e9fc commit f79e6d2
Show file tree
Hide file tree
Showing 5 changed files with 180 additions and 231 deletions.
11 changes: 2 additions & 9 deletions rcl/src/rcl/client.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,14 +90,9 @@ configure_service_introspection(
RCL_CHECK_FOR_NULL_WITH_MSG(
client_impl->service_event_publisher, "allocating memory failed", return RCL_RET_BAD_ALLOC;);

rcl_service_event_publisher_options_t service_event_options =
rcl_service_event_publisher_get_default_options();
service_event_options.publisher_options = options->event_publisher_options;
service_event_options.clock = options->clock;

*client_impl->service_event_publisher = rcl_get_zero_initialized_service_event_publisher();
rcl_ret_t ret = rcl_service_event_publisher_init(
client_impl->service_event_publisher, node, &service_event_options,
client_impl->service_event_publisher, node, options->clock, options->event_publisher_options,
remapped_service_name, type_support);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
Expand Down Expand Up @@ -501,9 +496,7 @@ rcl_service_introspection_configure_client_service_event_message_payload(
RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(client->impl, RCL_RET_INVALID_ARGUMENT);

client->impl->service_event_publisher->impl->options._content_enabled = enable;

return RCL_RET_OK;
return rcl_service_introspection_enable_content(client->impl->service_event_publisher, enable);
}

#ifdef __cplusplus
Expand Down
11 changes: 2 additions & 9 deletions rcl/src/rcl/service.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,14 +94,9 @@ configure_service_introspection(
RCL_CHECK_FOR_NULL_WITH_MSG(
service_impl->service_event_publisher, "allocating memory failed", return RCL_RET_BAD_ALLOC;);

rcl_service_event_publisher_options_t service_event_options =
rcl_service_event_publisher_get_default_options();
service_event_options.publisher_options = options->event_publisher_options;
service_event_options.clock = options->clock;

*service_impl->service_event_publisher = rcl_get_zero_initialized_service_event_publisher();
rcl_ret_t ret = rcl_service_event_publisher_init(
service_impl->service_event_publisher, node, &service_event_options,
service_impl->service_event_publisher, node, options->clock, options->event_publisher_options,
remapped_service_name, type_support);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
Expand Down Expand Up @@ -515,9 +510,7 @@ rcl_service_introspection_configure_server_service_event_message_payload(
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(service->impl, RCL_RET_INVALID_ARGUMENT);

service->impl->service_event_publisher->impl->options._content_enabled = enable;

return RCL_RET_OK;
return rcl_service_introspection_enable_content(service->impl->service_event_publisher, enable);
}

#ifdef __cplusplus
Expand Down
150 changes: 70 additions & 80 deletions rcl/src/rcl/service_event_publisher.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,30 +42,15 @@ rcl_service_event_publisher_t rcl_get_zero_initialized_service_event_publisher()
return zero_service_event_publisher;
}

rcl_service_event_publisher_options_t
rcl_service_event_publisher_get_default_options()
{
static rcl_service_event_publisher_options_t default_options;
// Must set the options after because they are not a compile time constant.
default_options._enabled = true;
default_options._content_enabled = true;
default_options.publisher_options = rcl_publisher_get_default_options();
default_options.clock = NULL;
return default_options;
}

bool
rcl_service_event_publisher_is_valid(const rcl_service_event_publisher_t * service_event_publisher)
{
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher, "service_event_publisher is invalid", return false);
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher->impl, "service_event_publisher's implementation is invalid",
return false);
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher->impl->service_type_support,
service_event_publisher->service_type_support,
"service_event_publisher's service type support is invalid", return false);
if (!rcl_clock_valid(service_event_publisher->impl->options.clock)) {
if (!rcl_clock_valid(service_event_publisher->clock)) {
RCL_SET_ERROR_MSG("service_event_publisher's clock is invalid");
return false;
}
Expand All @@ -75,7 +60,8 @@ rcl_service_event_publisher_is_valid(const rcl_service_event_publisher_t * servi
rcl_ret_t rcl_service_event_publisher_init(
rcl_service_event_publisher_t * service_event_publisher,
const rcl_node_t * node,
const rcl_service_event_publisher_options_t * options,
rcl_clock_t * clock,
const rcl_publisher_options_t publisher_options,
const char * service_name,
const rosidl_service_type_support_t * service_type_support)
{
Expand All @@ -89,60 +75,50 @@ rcl_ret_t rcl_service_event_publisher_init(

RCL_CHECK_ARGUMENT_FOR_NULL(service_event_publisher, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(service_type_support, RCL_RET_INVALID_ARGUMENT);

RCL_CHECK_ALLOCATOR_WITH_MSG(
&options->publisher_options.allocator,
&publisher_options.allocator,
"allocator is invalid", return RCL_RET_ERROR);
rcl_allocator_t allocator = options->publisher_options.allocator;
rcl_allocator_t allocator = publisher_options.allocator;

rcl_ret_t ret = RCL_RET_OK;

if (service_event_publisher->impl) {
rcutils_reset_error();
RCL_SET_ERROR_MSG("service event publisher already initialized, or memory was uninitialized");
return RCL_RET_ALREADY_INIT;
}

if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID;
}

if (!rcl_clock_valid(options->clock)) {
if (!rcl_clock_valid(clock)) {
rcutils_reset_error();
RCL_SET_ERROR_MSG("clock is invalid");
return RCL_RET_ERROR;
}

RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing service introspection for service name '%s'", service_name);
service_event_publisher->impl = (rcl_service_event_publisher_impl_t *) allocator.allocate(
sizeof(rcl_service_event_publisher_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher->impl, "allocating memory for rcl_service_event_publisher failed",
return RCL_RET_BAD_ALLOC;);

// Typesupports have static lifetimes
service_event_publisher->impl->service_type_support = service_type_support;
service_event_publisher->impl->options = *options;
service_event_publisher->service_type_support = service_type_support;
service_event_publisher->clock = clock;
service_event_publisher->publisher_options = publisher_options;

size_t topic_length = strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1;
service_event_publisher->impl->service_event_topic_name = (char *) allocator.allocate(
service_event_publisher->service_event_topic_name = (char *) allocator.allocate(
topic_length, allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher->impl->service_event_topic_name,
service_event_publisher->service_event_topic_name,
"allocating memory for service introspection topic name failed",
ret = RCL_RET_BAD_ALLOC; goto free_impl;);
return RCL_RET_BAD_ALLOC;);

snprintf(
service_event_publisher->impl->service_event_topic_name,
service_event_publisher->service_event_topic_name,
topic_length,
"%s%s", service_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX);

service_event_publisher->impl->options._enabled = false;
service_event_publisher->impl->publisher = NULL;
service_event_publisher->service_event_enabled = false;
service_event_publisher->service_event_content_enabled = false;
service_event_publisher->publisher = NULL;
ret = rcl_service_introspection_enable(service_event_publisher, node);
if (ret != RCL_RET_OK) {
goto free_topic_name;
Expand All @@ -153,11 +129,7 @@ rcl_ret_t rcl_service_event_publisher_init(
return RCL_RET_OK;

free_topic_name:
allocator.deallocate(service_event_publisher->impl->service_event_topic_name, allocator.state);

free_impl:
allocator.deallocate(service_event_publisher->impl, allocator.state);
service_event_publisher->impl = NULL;
allocator.deallocate(service_event_publisher->service_event_topic_name, allocator.state);

return ret;
}
Expand All @@ -181,13 +153,10 @@ rcl_ret_t rcl_service_event_publisher_fini(
rcutils_reset_error();
return ret;
}
rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator;
rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator;

allocator.deallocate(service_event_publisher->impl->service_event_topic_name, allocator.state);
service_event_publisher->impl->service_event_topic_name = NULL;

allocator.deallocate(service_event_publisher->impl, allocator.state);
service_event_publisher->impl = NULL;
allocator.deallocate(service_event_publisher->service_event_topic_name, allocator.state);
service_event_publisher->service_event_topic_name = NULL;

return RCL_RET_OK;
}
Expand All @@ -212,21 +181,21 @@ rcl_ret_t rcl_send_service_event_message(
}

// early exit if service introspection disabled during runtime
if (!service_event_publisher->impl->options._enabled) {
if (!service_event_publisher->service_event_enabled) {
return RCL_RET_OK;
}

rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator;
rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator;
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);

if (!rcl_publisher_is_valid(service_event_publisher->impl->publisher)) {
if (!rcl_publisher_is_valid(service_event_publisher->publisher)) {
return RCL_RET_PUBLISHER_INVALID;
}

rcl_ret_t ret;

rcl_time_point_value_t now;
ret = rcl_clock_get_now(service_event_publisher->impl->options.clock, &now);
ret = rcl_clock_get_now(service_event_publisher->clock, &now);
if (RMW_RET_OK != ret) {
rcutils_reset_error();
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
Expand All @@ -243,20 +212,20 @@ rcl_ret_t rcl_send_service_event_message(
memcpy(info.client_gid, guid, 16);

void * service_introspection_message;
if (!service_event_publisher->impl->options._content_enabled) {
if (!service_event_publisher->service_event_content_enabled) {
ros_response_request = NULL;
}
switch (event_type) {
case service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED:
case service_msgs__msg__ServiceEventInfo__REQUEST_SENT:
service_introspection_message =
service_event_publisher->impl->service_type_support->event_message_create_handle_function(
service_event_publisher->service_type_support->event_message_create_handle_function(
&info, &allocator, ros_response_request, NULL);
break;
case service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED:
case service_msgs__msg__ServiceEventInfo__RESPONSE_SENT:
service_introspection_message =
service_event_publisher->impl->service_type_support->event_message_create_handle_function(
service_event_publisher->service_type_support->event_message_create_handle_function(
&info, &allocator, NULL, ros_response_request);
break;
default:
Expand All @@ -269,9 +238,9 @@ rcl_ret_t rcl_send_service_event_message(

// and publish it out!
// TODO(ihasdapie): Publisher context can become invalidated on shutdown
ret = rcl_publish(service_event_publisher->impl->publisher, service_introspection_message, NULL);
ret = rcl_publish(service_event_publisher->publisher, service_introspection_message, NULL);
// clean up before error checking
service_event_publisher->impl->service_type_support->event_message_destroy_handle_function(
service_event_publisher->service_type_support->event_message_destroy_handle_function(
service_introspection_message, &allocator);
if (RCL_RET_OK != ret) {
rcutils_reset_error();
Expand Down Expand Up @@ -301,33 +270,34 @@ rcl_ret_t rcl_service_introspection_enable(
// need to check if node_opt is disabled

// Early exit if already enabled
if (service_event_publisher->impl->options._enabled) {
if (service_event_publisher->service_event_enabled) {
return RCL_RET_OK;
}

rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator;
rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator;
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_ERROR);

service_event_publisher->impl->publisher = allocator.allocate(
service_event_publisher->publisher = allocator.allocate(
sizeof(rcl_publisher_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher->impl->publisher,
service_event_publisher->publisher,
"allocate service_event_publisher failed in enable", return RCL_RET_BAD_ALLOC);
*service_event_publisher->impl->publisher = rcl_get_zero_initialized_publisher();
*service_event_publisher->publisher = rcl_get_zero_initialized_publisher();
rcl_ret_t ret = rcl_publisher_init(
service_event_publisher->impl->publisher, node,
service_event_publisher->impl->service_type_support->event_typesupport,
service_event_publisher->impl->service_event_topic_name,
&service_event_publisher->impl->options.publisher_options);
service_event_publisher->publisher, node,
service_event_publisher->service_type_support->event_typesupport,
service_event_publisher->service_event_topic_name,
&service_event_publisher->publisher_options);
if (RCL_RET_OK != ret) {
allocator.deallocate(service_event_publisher->impl->publisher, allocator.state);
service_event_publisher->impl->publisher = NULL;
allocator.deallocate(service_event_publisher->publisher, allocator.state);
service_event_publisher->publisher = NULL;
rcutils_reset_error();
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
return ret;
}

service_event_publisher->impl->options._enabled = true;
service_event_publisher->service_event_enabled = true;

return RCL_RET_OK;
}

Expand All @@ -350,24 +320,44 @@ rcl_ret_t rcl_service_introspection_disable(
}

// Early exit if already disabled
if (!service_event_publisher->impl->options._enabled) {
if (!service_event_publisher->service_event_enabled) {
return RCL_RET_OK;
}

rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator;
rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator;
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_ERROR);

if (service_event_publisher->impl->publisher) {
rcl_ret_t ret = rcl_publisher_fini(service_event_publisher->impl->publisher, node);
allocator.deallocate(service_event_publisher->impl->publisher, allocator.state);
service_event_publisher->impl->publisher = NULL;
if (service_event_publisher->publisher) {
rcl_ret_t ret = rcl_publisher_fini(service_event_publisher->publisher, node);
allocator.deallocate(service_event_publisher->publisher, allocator.state);
service_event_publisher->publisher = NULL;
if (RCL_RET_OK != ret) {
rcutils_reset_error();
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
return ret;
}
}

service_event_publisher->impl->options._enabled = false;
service_event_publisher->service_event_enabled = false;
service_event_publisher->service_event_content_enabled = false;

return RCL_RET_OK;
}

rcl_ret_t rcl_service_introspection_enable_content(
rcl_service_event_publisher_t * service_event_publisher,
bool enable)
{
if (!rcl_service_event_publisher_is_valid(service_event_publisher)) {
return RCL_RET_ERROR;
}

if (!service_event_publisher->service_event_enabled) {
// Enabling the content without having the service_event enabled is incoherent
return RCL_RET_ERROR;
}

service_event_publisher->service_event_content_enabled = enable;

return RCL_RET_OK;
}
Loading

0 comments on commit f79e6d2

Please sign in to comment.