Skip to content

Commit

Permalink
More cleanup.
Browse files Browse the repository at this point in the history 
Signed-off-by: Chris Lalancette <clalancette@gmail.com>
  • Loading branch information
@clalancette
clalancette committed Feb 17, 2023
1 parent a32209d commit 400b531
Show file tree
Hide file tree
Showing 3 changed files with 127 additions and 85 deletions.
8 changes: 4 additions & 4 deletions rcl/src/rcl/client.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -344,7 +344,7 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t
rmw_ret_t rmw_ret = rmw_get_gid_for_client(client->impl->rmw_handle, &gid);
if (rmw_ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
return RCL_RET_ERROR;
return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
}
rcl_ret_t ret = rcl_send_service_event_message(
client->impl->service_event_publisher,
Expand All @@ -354,7 +354,7 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t
gid.data);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
return RCL_RET_ERROR;
return ret;
}
}
return RCL_RET_OK;
Expand Down Expand Up @@ -394,7 +394,7 @@ rcl_take_response_with_info(
rmw_ret_t rmw_ret = rmw_get_gid_for_client(client->impl->rmw_handle, &gid);
if (rmw_ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
return RCL_RET_ERROR;
return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
}
rcl_ret_t ret = rcl_send_service_event_message(
client->impl->service_event_publisher,
Expand All @@ -404,7 +404,7 @@ rcl_take_response_with_info(
gid.data);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
return RCL_RET_ERROR;
return ret;
}
}
return RCL_RET_OK;
Expand Down
158 changes: 104 additions & 54 deletions rcl/src/rcl/service.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,14 +30,13 @@ extern "C"
#include "rcutils/macros.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"

#include "service_msgs/msg/service_event_info.h"
#include "tracetools/tracetools.h"

#include "rosidl_runtime_c/service_type_support_struct.h"
#include "rosidl_runtime_c/message_type_support_struct.h"

#include "service_msgs/msg/service_event_info.h"

#include "./common.h"
#include "./service_event_publisher.h"
#include "./service_impl.h"

Expand All @@ -49,6 +48,63 @@ rcl_get_zero_initialized_service()
return null_service;
}

static
rcl_ret_t
unconfigure_service_introspection(
rcl_node_t * node,
struct rcl_service_impl_s * service_impl,
rcl_allocator_t * allocator)
{
if (!service_impl->service_event_publisher) {
return RCL_RET_OK;
}

rcl_ret_t ret = rcl_service_event_publisher_fini(service_impl->service_event_publisher, node);

allocator->deallocate(service_impl->service_event_publisher, allocator->state);
service_impl->service_event_publisher = NULL;

return ret;
}

static
rcl_ret_t
configure_service_introspection(
const rcl_node_t * node,
struct rcl_service_impl_s * service_impl,
rcl_allocator_t * allocator,
const rcl_service_options_t * options,
const rosidl_service_type_support_t * type_support,
const char * remapped_service_name)
{
if (!rcl_node_get_options(node)->enable_service_introspection) {
return RCL_RET_OK;
}

service_impl->service_event_publisher = allocator->zero_allocate(
1, sizeof(rcl_service_event_publisher_t), allocator->state);
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,
remapped_service_name, type_support);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
allocator->deallocate(service_impl->service_event_publisher, allocator->state);
service_impl->service_event_publisher = NULL;
return ret;
}

return RCL_RET_OK;
}

rcl_ret_t
rcl_service_init(
rcl_service_t * service,
Expand Down Expand Up @@ -99,7 +155,7 @@ rcl_service_init(
} else if (ret != RCL_RET_BAD_ALLOC) {
ret = RCL_RET_ERROR;
}
goto cleanup;
return ret;
}
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name);
Expand All @@ -108,7 +164,8 @@ rcl_service_init(
service->impl = (rcl_service_impl_t *)allocator->zero_allocate(
1, 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);
service->impl, "allocating memory failed",
ret = RCL_RET_BAD_ALLOC; goto free_remapped_service_name);

if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
RCUTILS_LOG_WARN_NAMED(
Expand All @@ -126,47 +183,33 @@ rcl_service_init(
&options->qos);
if (!service->impl->rmw_handle) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
goto fail;
ret = RCL_RET_ERROR;
goto free_service_impl;
}

if (rcl_node_get_options(node)->enable_service_introspection) {
service->impl->service_event_publisher = allocator->zero_allocate(
1, sizeof(rcl_service_event_publisher_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
service->impl->service_event_publisher,
"allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto fail);

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();
ret = rcl_service_event_publisher_init(
service->impl->service_event_publisher, node, &service_event_options,
remapped_service_name, type_support);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
goto fail;
}
ret = configure_service_introspection(
node, service->impl, allocator, options, type_support, remapped_service_name);
if (RCL_RET_OK != ret) {
goto destroy_service;
}

// get actual qos, and store it
rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos(
service->impl->rmw_handle,
&service->impl->actual_request_subscription_qos);

if (RMW_RET_OK != rmw_ret) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
goto fail;
ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
goto unconfigure_introspection;
}

rmw_ret = rmw_service_response_publisher_get_actual_qos(
service->impl->rmw_handle,
&service->impl->actual_response_publisher_qos);

if (RMW_RET_OK != rmw_ret) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
goto fail;
ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
goto unconfigure_introspection;
}

// ROS specific namespacing conventions is not retrieved by get_actual_qos
Expand All @@ -185,19 +228,29 @@ rcl_service_init(
(const void *)node,
(const void *)service->impl->rmw_handle,
remapped_service_name);
goto cleanup;
fail:
if (service->impl) {
if (service->impl->service_event_publisher) {
allocator->deallocate(service->impl->service_event_publisher, allocator->state);
service->impl->service_event_publisher = NULL;
}
allocator->deallocate(service->impl, allocator->state);
service->impl = NULL;

goto free_remapped_service_name;

unconfigure_introspection:
// TODO(clalancette): I don't love casting away the const from node here,
// but the cleanup path goes deep and I didn't want to change 6 or so
// different signatures.
fail_ret = unconfigure_service_introspection((rcl_node_t *)node, service->impl, allocator);
if (RCL_RET_OK != fail_ret) {
// TODO(clalancette): print the error message here
}

destroy_service:
rmw_ret = rmw_destroy_service(rcl_node_get_rmw_handle(node), service->impl->rmw_handle);
if (RMW_RET_OK != rmw_ret) {
// TODO(clalancette): print the error message here
}
ret = fail_ret;
// Fall through to clean up
cleanup:

free_service_impl:
allocator->deallocate(service->impl, allocator->state);
service->impl = NULL;

free_remapped_service_name:
allocator->deallocate(remapped_service_name, allocator->state);
return ret;
}
Expand All @@ -223,22 +276,19 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
if (!rmw_node) {
return RCL_RET_INVALID_ARGUMENT;
}

rcl_ret_t rcl_ret = unconfigure_service_introspection(node, service->impl, &allocator);
if (RCL_RET_OK != rcl_ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
result = rcl_ret;
}

rmw_ret_t ret = rmw_destroy_service(rmw_node, service->impl->rmw_handle);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
result = RCL_RET_ERROR;
}

if (service->impl->service_event_publisher) {
ret = rcl_service_event_publisher_fini(service->impl->service_event_publisher, node);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
result = ret;
}
allocator.deallocate(service->impl->service_event_publisher, allocator.state);
service->impl->service_event_publisher = NULL;
}

allocator.deallocate(service->impl, allocator.state);
service->impl = NULL;
}
Expand Down Expand Up @@ -330,7 +380,7 @@ rcl_take_request_with_info(
request_header->request_id.writer_guid);
if (RCL_RET_OK != rclret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
return RCL_RET_ERROR;
return rclret;
}
}
return RCL_RET_OK;
Expand Down Expand Up @@ -381,7 +431,7 @@ rcl_send_response(
request_header->writer_guid);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
return RCL_RET_ERROR;
return ret;
}
}
return RCL_RET_OK;
Expand Down
46 changes: 19 additions & 27 deletions rcl/src/rcl/service_event_publisher.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ rcl_ret_t rcl_service_event_publisher_init(
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",
goto fail;);
return RCL_RET_BAD_ALLOC;);

// Typesupports have static lifetimes
service_event_publisher->impl->service_type_support = service_type_support;
Expand All @@ -134,7 +134,7 @@ rcl_ret_t rcl_service_event_publisher_init(
RCL_CHECK_FOR_NULL_WITH_MSG(
service_event_publisher->impl->service_event_topic_name,
"allocating memory for service introspection topic name failed",
goto fail;);
ret = RCL_RET_BAD_ALLOC; goto free_impl;);

snprintf(
service_event_publisher->impl->service_event_topic_name,
Expand All @@ -145,28 +145,21 @@ rcl_ret_t rcl_service_event_publisher_init(
service_event_publisher->impl->publisher = NULL;
ret = rcl_service_introspection_enable(service_event_publisher, node);
if (ret != RCL_RET_OK) {
goto fail;
goto free_topic_name;
}

RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Service introspection for service '%s' initialized", service_name);
return RCL_RET_OK;

fail:
if (service_event_publisher->impl) {
if (service_event_publisher->impl->publisher) {
allocator.deallocate(service_event_publisher->impl->publisher, allocator.state);
service_event_publisher->impl->publisher = NULL;
}
allocator.deallocate(service_event_publisher->impl->service_event_topic_name, allocator.state);
allocator.deallocate(service_event_publisher->impl, allocator.state);
service_event_publisher->impl = NULL;
}
if (RCL_RET_OK != ret) {
rcutils_reset_error();
return ret;
}
return RCL_RET_ERROR;
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;

return ret;
}

rcl_ret_t rcl_service_event_publisher_fini(
Expand Down Expand Up @@ -247,9 +240,7 @@ rcl_ret_t rcl_send_service_event_message(
.sequence_number = sequence_number,
};

for (size_t i = 0; i < 16; ++i) {
info.client_gid[i] = guid[i];
}
memcpy(info.client_gid, guid, 16);

void * service_introspection_message;
if (!service_event_publisher->impl->options._content_enabled) {
Expand Down Expand Up @@ -279,16 +270,15 @@ 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);
if (RMW_RET_OK != ret) {
// clean up before error checking
service_event_publisher->impl->service_type_support->event_message_destroy_handle_function(
service_introspection_message, &allocator);
if (RCL_RET_OK != ret) {
rcutils_reset_error();
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
return ret;
}

// clean up
service_event_publisher->impl->service_type_support->event_message_destroy_handle_function(
service_introspection_message, &allocator);
return RCL_RET_OK;
return ret;
}

rcl_ret_t rcl_service_introspection_enable(
Expand Down Expand Up @@ -330,6 +320,8 @@ rcl_ret_t rcl_service_introspection_enable(
service_event_publisher->impl->service_event_topic_name,
&service_event_publisher->impl->options.publisher_options);
if (RCL_RET_OK != ret) {
allocator.deallocate(service_event_publisher->impl->publisher, allocator.state);
service_event_publisher->impl->publisher = NULL;
rcutils_reset_error();
RCL_SET_ERROR_MSG(rcl_get_error_string().str);
return ret;
Expand Down

0 comments on commit 400b531

Please sign in to comment.