Skip to content

Commit

Permalink
Remove vestigial node option for enable_service_introspection.
Browse files Browse the repository at this point in the history
Also a couple of other fixes from review.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
  • Loading branch information
clalancette committed Feb 21, 2023
1 parent c12351c commit 2f1e9fc
Show file tree
Hide file tree
Showing 8 changed files with 27 additions and 15 deletions.
5 changes: 3 additions & 2 deletions rcl/include/rcl/client.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,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
/// Enable/Disable service introspection
bool enable_service_introspection;
/// The clock to use for service introspection message timestampes
/// The clock to use for service introspection message timestamps; if
/// enable_service_introspection is true, must not be NULL
rcl_clock_t * clock;
} rcl_client_options_t;

Expand Down
3 changes: 0 additions & 3 deletions rcl/include/rcl/node_options.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,6 @@ typedef struct rcl_node_options_s

/// Middleware quality of service settings for /rosout.
rmw_qos_profile_t rosout_qos;

/// Flag to enable introspection features for services related to this node
bool enable_service_introspection;
} rcl_node_options_t;

/// Return the default node options in a rcl_node_options_t.
Expand Down
3 changes: 2 additions & 1 deletion rcl/include/rcl/service.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,8 @@ typedef struct rcl_service_options_s
rcl_allocator_t allocator;
/// Enable/Disable service introspection features
bool enable_service_introspection;
/// The clock to use for service introspection message timestamps
/// The clock to use for service introspection message timestamps; if
/// enable_service_introspection is true, must not be NULL
rcl_clock_t * clock;
} rcl_service_options_t;

Expand Down
14 changes: 11 additions & 3 deletions rcl/src/rcl/client.c
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,10 @@ unconfigure_service_introspection(
struct rcl_client_impl_s * client_impl,
rcl_allocator_t * allocator)
{
if (client_impl == NULL) {
return RCL_RET_ERROR;
}

if (!client_impl->service_event_publisher) {
return RCL_RET_OK;
}
Expand All @@ -73,12 +77,16 @@ configure_service_introspection(
const rosidl_service_type_support_t * type_support,
const char * remapped_service_name)
{
if (!rcl_node_get_options(node)->enable_service_introspection) {
if (!options->enable_service_introspection) {
return RCL_RET_OK;
}

client_impl->service_event_publisher = allocator->zero_allocate(
1, sizeof(rcl_service_event_publisher_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
options->clock, "Clock must not be NULL when service introspection is enabled",
return RCL_RET_ERROR;);

client_impl->service_event_publisher = allocator->allocate(
sizeof(rcl_service_event_publisher_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
client_impl->service_event_publisher, "allocating memory failed", return RCL_RET_BAD_ALLOC;);

Expand Down
2 changes: 0 additions & 2 deletions rcl/src/rcl/node_options.c
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@ rcl_node_get_default_options()
.arguments = rcl_get_zero_initialized_arguments(),
.enable_rosout = true,
.rosout_qos = rcl_qos_profile_rosout_default,
.enable_service_introspection = false,
};
return default_options;
}
Expand All @@ -62,7 +61,6 @@ rcl_node_options_copy(
options_out->use_global_arguments = options->use_global_arguments;
options_out->enable_rosout = options->enable_rosout;
options_out->rosout_qos = options->rosout_qos;
options_out->enable_service_introspection = options->enable_service_introspection;
if (NULL != options->arguments.impl) {
return rcl_arguments_copy(&(options->arguments), &(options_out->arguments));
}
Expand Down
10 changes: 9 additions & 1 deletion rcl/src/rcl/service.c
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,10 @@ unconfigure_service_introspection(
struct rcl_service_impl_s * service_impl,
rcl_allocator_t * allocator)
{
if (service_impl == NULL) {
return RCL_RET_ERROR;
}

if (!service_impl->service_event_publisher) {
return RCL_RET_OK;
}
Expand All @@ -77,10 +81,14 @@ configure_service_introspection(
const rosidl_service_type_support_t * type_support,
const char * remapped_service_name)
{
if (!rcl_node_get_options(node)->enable_service_introspection) {
if (!options->enable_service_introspection) {
return RCL_RET_OK;
}

RCL_CHECK_FOR_NULL_WITH_MSG(
options->clock, "Clock must not be NULL when service introspection is enabled",
return RCL_RET_ERROR;);

service_impl->service_event_publisher = allocator->zero_allocate(
1, sizeof(rcl_service_event_publisher_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
Expand Down
2 changes: 1 addition & 1 deletion rcl/src/rcl/service_event_publisher.c
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ rcl_ret_t rcl_service_event_publisher_init(

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

Expand Down
3 changes: 1 addition & 2 deletions rcl/test/rcl/test_service_event_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ class CLASSNAME (TestServiceEventPublisherFixture, RMW_IMPLEMENTATION) : public
rcl_ret_t ret;
const rosidl_service_type_support_t * srv_ts =
ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes);

void SetUp() override
{
rcl_ret_t ret;
Expand All @@ -74,7 +75,6 @@ class CLASSNAME (TestServiceEventPublisherFixture, RMW_IMPLEMENTATION) : public
*this->node_ptr = rcl_get_zero_initialized_node();
const char * name = "test_service_event_publisher_node";
rcl_node_options_t node_options = rcl_node_get_default_options();
node_options.enable_service_introspection = true;
ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
Expand Down Expand Up @@ -428,7 +428,6 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP
*this->node_ptr = rcl_get_zero_initialized_node();
const char * name = "test_service_node";
rcl_node_options_t node_options = rcl_node_get_default_options();
node_options.enable_service_introspection = true;
ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
const rosidl_service_type_support_t * srv_ts =
Expand Down

0 comments on commit 2f1e9fc

Please sign in to comment.