niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add debug logging (#187)

Browse source 
* use ROS_PACKAGE_NAME in debug msgs

* rcl_lifecycle too

* Swap unnamed macros to named

* Remove semicolon

* Add debug logging

* Timer debug logging

* Wait debug

* A bit less wait debug...

* Clearer time output

* Remove the wait sublogger

* Use conditional logging instead of the else{}

* Add 'X finalized' msg

* Add send_response logging

* Remove extra semicolons

* Add publish/take messages

* [style nitpick] formatted variables on the next line
This commit is contained in:
dhood 2017-11-21 17:23:50 -08:00 • committed by GitHub
parent 9f92f8fa37
commit 7d0045adb8
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
18 changed files with 186 additions and 73 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rcl/include/rcl/expand_topic_name.h
View file

@ -64,7 +64,7 @@ extern "C"
* if (ret != RCL_RET_OK) { * if (ret != RCL_RET_OK) {
* // ... error handling * // ... error handling
* } else { * } else {
* RCUTILS_LOG_INFO("Expanded topic name: %s", expanded_topic_name) * RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Expanded topic name: %s", expanded_topic_name)
* // ... when done the output topic name needs to be deallocated: * // ... when done the output topic name needs to be deallocated:
* allocator.deallocate(expanded_topic_name, allocator.state); * allocator.deallocate(expanded_topic_name, allocator.state);
* } * }

12
rcl/src/rcl/client.c
View file

@ -67,6 +67,8 @@ rcl_client_init(
} }
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing client for service name '%s'", service_name)
if (client->impl) { if (client->impl) {
RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized", *allocator); RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized", *allocator);
return RCL_RET_ALREADY_INIT; return RCL_RET_ALREADY_INIT;
@ -87,7 +89,7 @@ rcl_client_init(
rcutils_ret = rcutils_string_map_fini(&substitutions_map); rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) { if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"failed to fini string_map (%d) during error handling: %s\n", "failed to fini string_map (%d) during error handling: %s\n",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()) rcutils_get_error_string_safe())
@ -120,6 +122,7 @@ rcl_client_init(
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name)
// Validate the expanded service name. // Validate the expanded service name.
int validation_result; int validation_result;
rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL); rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL);
@ -151,6 +154,7 @@ rcl_client_init(
} }
// options // options
client->impl->options = *options; client->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized")
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (client->impl) { if (client->impl) {
@ -163,6 +167,7 @@ rcl_ret_t
rcl_client_fini(rcl_client_t * client, rcl_node_t * node) rcl_client_fini(rcl_client_t * client, rcl_node_t * node)
{ {
(void)node; (void)node;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing client")
rcl_ret_t result = RCL_RET_OK; rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@ -182,6 +187,7 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node)
} }
allocator.deallocate(client->impl, allocator.state); allocator.deallocate(client->impl, allocator.state);
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client finalized")
return result; return result;
} }
@ -232,6 +238,7 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number) rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client sending service request")
if (!rcl_client_is_valid(client)) { if (!rcl_client_is_valid(client)) {
return RCL_RET_CLIENT_INVALID; return RCL_RET_CLIENT_INVALID;
} }
@ -257,6 +264,7 @@ rcl_take_response(
rmw_request_id_t * request_header, rmw_request_id_t * request_header,
void * ros_response) void * ros_response)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response")
if (!rcl_client_is_valid(client)) { if (!rcl_client_is_valid(client)) {
return RCL_RET_CLIENT_INVALID; return RCL_RET_CLIENT_INVALID;
} }
@ -272,6 +280,8 @@ rcl_take_response(
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), client->impl->options.allocator); RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), client->impl->options.allocator);
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Client take response succeeded: %s", taken ? "true" : "false")
if (!taken) { if (!taken) {
return RCL_RET_CLIENT_TAKE_FAILED; return RCL_RET_CLIENT_TAKE_FAILED;
} }

12
rcl/src/rcl/node.c
View file

@ -108,6 +108,8 @@ rcl_node_init(
RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing node '%s' in namespace '%s'", name, namespace_)
if (node->impl) { if (node->impl) {
RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized", *allocator); RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized", *allocator);
return RCL_RET_ALREADY_INIT; return RCL_RET_ALREADY_INIT;
@ -215,6 +217,7 @@ rcl_node_init(
} }
// actual domain id // actual domain id
node->impl->actual_domain_id = domain_id; node->impl->actual_domain_id = domain_id;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Using domain ID of '%zu'", domain_id)
const char * ros_security_enable = NULL; const char * ros_security_enable = NULL;
const char * ros_enforce_security = NULL; const char * ros_enforce_security = NULL;
@ -230,6 +233,8 @@ rcl_node_init(
} }
bool use_security = (0 == strcmp(ros_security_enable, "true")); bool use_security = (0 == strcmp(ros_security_enable, "true"));
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Using security: %s", use_security ? "true" : "false")
if (rcutils_get_env(ROS_SECURITY_STRATEGY_VAR_NAME, &ros_enforce_security)) { if (rcutils_get_env(ROS_SECURITY_STRATEGY_VAR_NAME, &ros_enforce_security)) {
RCL_SET_ERROR_MSG( RCL_SET_ERROR_MSG(
@ -301,6 +306,7 @@ rcl_node_init(
// error message already set // error message already set
goto fail; goto fail;
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized")
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (node->impl) { if (node->impl) {
@ -308,7 +314,7 @@ fail:
ret = rmw_destroy_node(node->impl->rmw_node_handle); ret = rmw_destroy_node(node->impl->rmw_node_handle);
if (ret != RMW_RET_OK) { if (ret != RMW_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"failed to fini rmw node in error recovery: %s", rmw_get_error_string_safe() "failed to fini rmw node in error recovery: %s", rmw_get_error_string_safe()
) )
} }
@ -317,7 +323,7 @@ fail:
ret = rcl_guard_condition_fini(node->impl->graph_guard_condition); ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe() "failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe()
) )
} }
@ -337,6 +343,7 @@ fail:
rcl_ret_t rcl_ret_t
rcl_node_fini(rcl_node_t * node) rcl_node_fini(rcl_node_t * node)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing node")
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (!node->impl) { if (!node->impl) {
// Repeat calls to fini or calling fini on a zero initialized node is ok. // Repeat calls to fini or calling fini on a zero initialized node is ok.
@ -358,6 +365,7 @@ rcl_node_fini(rcl_node_t * node)
// assuming that allocate and deallocate are ok since they are checked in init // assuming that allocate and deallocate are ok since they are checked in init
allocator.deallocate(node->impl, allocator.state); allocator.deallocate(node->impl, allocator.state);
node->impl = NULL; node->impl = NULL;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized")
return result; return result;
} }

9
rcl/src/rcl/publisher.c
View file

@ -70,6 +70,8 @@ rcl_publisher_init(
} }
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing publisher for topic name '%s'", topic_name)
// Expand the given topic name. // Expand the given topic name.
rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version
rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
@ -86,7 +88,7 @@ rcl_publisher_init(
rcutils_ret = rcutils_string_map_fini(&substitutions_map); rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) { if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"failed to fini string_map (%d) during error handling: %s", "failed to fini string_map (%d) during error handling: %s",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()) rcutils_get_error_string_safe())
@ -119,6 +121,7 @@ rcl_publisher_init(
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name)
// Validate the expanded topic name. // Validate the expanded topic name.
int validation_result; int validation_result;
rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL); rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL);
@ -148,6 +151,7 @@ rcl_publisher_init(
rmw_get_error_string_safe(), goto fail, *allocator); rmw_get_error_string_safe(), goto fail, *allocator);
// options // options
publisher->impl->options = *options; publisher->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized")
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (publisher->impl) { if (publisher->impl) {
@ -165,6 +169,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
if (!rcl_node_is_valid(node, NULL)) { if (!rcl_node_is_valid(node, NULL)) {
return RCL_RET_NODE_INVALID; return RCL_RET_NODE_INVALID;
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing publisher")
if (publisher->impl) { if (publisher->impl) {
rcl_allocator_t allocator = publisher->impl->options.allocator; rcl_allocator_t allocator = publisher->impl->options.allocator;
rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node);
@ -179,6 +184,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
} }
allocator.deallocate(publisher->impl, allocator.state); allocator.deallocate(publisher->impl, allocator.state);
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher finalized")
return result; return result;
} }
@ -196,6 +202,7 @@ rcl_publisher_get_default_options()
rcl_ret_t rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message) rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher publishing message")
if (!rcl_publisher_is_valid(publisher)) { if (!rcl_publisher_is_valid(publisher)) {
return RCL_RET_PUBLISHER_INVALID; return RCL_RET_PUBLISHER_INVALID;
} }

2
rcl/src/rcl/rcl.c
View file

@ -23,6 +23,7 @@ extern "C"
#include "./stdatomic_helper.h" #include "./stdatomic_helper.h"
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false); static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false);
@ -116,6 +117,7 @@ fail:
rcl_ret_t rcl_ret_t
rcl_shutdown() rcl_shutdown()
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Shutting down")
if (!rcl_ok()) { if (!rcl_ok()) {
// must use default allocator here because __rcl_allocator may not be set yet // must use default allocator here because __rcl_allocator may not be set yet
RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init", rcl_get_default_allocator()); RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init", rcl_get_default_allocator());

4
rcl/src/rcl/rmw_implementation_identifier_check.c
View file

@ -57,7 +57,7 @@ INITIALIZER(initialize) {
rcl_ret_t ret = rcl_impl_getenv("RCL_ASSERT_RMW_ID_MATCHES", &expected); rcl_ret_t ret = rcl_impl_getenv("RCL_ASSERT_RMW_ID_MATCHES", &expected);
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s", "Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s",
rcl_get_error_string_safe() rcl_get_error_string_safe()
) )
@ -66,7 +66,7 @@ INITIALIZER(initialize) {
// If the environment variable is set, and it does not match, print a warning and exit. // If the environment variable is set, and it does not match, print a warning and exit.
if (strlen(expected) > 0 && strcmp(rmw_get_implementation_identifier(), expected) != 0) { if (strlen(expected) > 0 && strcmp(rmw_get_implementation_identifier(), expected) != 0) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.", "Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.",
expected, expected,
rmw_get_implementation_identifier(), rmw_get_implementation_identifier(),

15
rcl/src/rcl/service.c
View file

@ -64,6 +64,8 @@ rcl_service_init(
} }
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing service for service name '%s'", service_name)
if (service->impl) { if (service->impl) {
RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized", *allocator); RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized", *allocator);
return RCL_RET_ALREADY_INIT; return RCL_RET_ALREADY_INIT;
@ -84,7 +86,7 @@ rcl_service_init(
rcutils_ret = rcutils_string_map_fini(&substitutions_map); rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) { if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"failed to fini string_map (%d) during error handling: %s", "failed to fini string_map (%d) during error handling: %s",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()) rcutils_get_error_string_safe())
@ -117,6 +119,7 @@ rcl_service_init(
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name)
// Validate the expanded service name. // Validate the expanded service name.
int validation_result; int validation_result;
rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL); rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL);
@ -136,7 +139,7 @@ rcl_service_init(
if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) { if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
RCUTILS_LOG_WARN_NAMED( RCUTILS_LOG_WARN_NAMED(
"rcl", ROS_PACKAGE_NAME,
"Warning: Setting QoS durability to 'transient local' for service servers " "Warning: Setting QoS durability to 'transient local' for service servers "
"can cause them to receive requests from clients that have since terminated.") "can cause them to receive requests from clients that have since terminated.")
} }
@ -155,6 +158,7 @@ rcl_service_init(
} }
// options // options
service->impl->options = *options; service->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized")
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (service->impl) { if (service->impl) {
@ -166,6 +170,7 @@ fail:
rcl_ret_t rcl_ret_t
rcl_service_fini(rcl_service_t * service, rcl_node_t * node) rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing service")
rcl_ret_t result = RCL_RET_OK; rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@ -185,6 +190,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
} }
allocator.deallocate(service->impl, allocator.state); allocator.deallocate(service->impl, allocator.state);
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized")
return result; return result;
} }
@ -239,6 +245,7 @@ rcl_take_request(
rmw_request_id_t * request_header, rmw_request_id_t * request_header,
void * ros_request) void * ros_request)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service server taking service request")
const rcl_service_options_t * options = rcl_service_get_options(service); const rcl_service_options_t * options = rcl_service_get_options(service);
RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator); RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT, options->allocator); RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT, options->allocator);
@ -250,6 +257,8 @@ rcl_take_request(
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator); RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Service take request succeeded: %s", taken ? "true" : "false")
if (!taken) { if (!taken) {
return RCL_RET_SERVICE_TAKE_FAILED; return RCL_RET_SERVICE_TAKE_FAILED;
} }
@ -262,6 +271,7 @@ rcl_send_response(
rmw_request_id_t * request_header, rmw_request_id_t * request_header,
void * ros_response) void * ros_response)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response")
const rcl_service_options_t * options = rcl_service_get_options(service); const rcl_service_options_t * options = rcl_service_get_options(service);
RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator); RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT, options->allocator); RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT, options->allocator);
@ -272,7 +282,6 @@ rcl_send_response(
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator); RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
return RCL_RET_OK; return RCL_RET_OK;
} }

14
rcl/src/rcl/stdatomic_helper/win32/stdatomic.h
View file

@ -209,7 +209,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_compare_exchange_strong") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_compare_exchange_strong") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -238,7 +238,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_exchange_strong") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_exchange_strong") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -264,7 +264,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_add") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_add") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -290,7 +290,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_and") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_and") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -316,7 +316,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_or") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_or") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -345,7 +345,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_xor") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_xor") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -371,7 +371,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
break; \ break; \
default: \ default: \
RCUTILS_LOG_ERROR_NAMED( \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_load") \ ROS_PACKAGE_NAME, "Unsupported integer type in atomic_load") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \

11
rcl/src/rcl/subscription.c
View file

@ -63,6 +63,8 @@ rcl_subscription_init(
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing subscription for topic name '%s'", topic_name)
if (subscription->impl) { if (subscription->impl) {
RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized", *allocator); RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized", *allocator);
return RCL_RET_ALREADY_INIT; return RCL_RET_ALREADY_INIT;
@ -83,7 +85,7 @@ rcl_subscription_init(
rcutils_ret = rcutils_string_map_fini(&substitutions_map); rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) { if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl", ROS_PACKAGE_NAME,
"failed to fini string_map (%d) during error handling: %s", "failed to fini string_map (%d) during error handling: %s",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()) rcutils_get_error_string_safe())
@ -116,6 +118,7 @@ rcl_subscription_init(
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name)
// Validate the expanded topic name. // Validate the expanded topic name.
int validation_result; int validation_result;
rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL); rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL);
@ -148,6 +151,7 @@ rcl_subscription_init(
} }
// options // options
subscription->impl->options = *options; subscription->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized")
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (subscription->impl) { if (subscription->impl) {
@ -159,6 +163,7 @@ fail:
rcl_ret_t rcl_ret_t
rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing subscription")
rcl_ret_t result = RCL_RET_OK; rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@ -179,6 +184,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
} }
allocator.deallocate(subscription->impl, allocator.state); allocator.deallocate(subscription->impl, allocator.state);
} }
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription finalized")
return result; return result;
} }
@ -201,6 +207,7 @@ rcl_take(
void * ros_message, void * ros_message,
rmw_message_info_t * message_info) rmw_message_info_t * message_info)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message")
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription); const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription);
if (!options) { if (!options) {
@ -224,6 +231,8 @@ rcl_take(
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator); RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false")
if (!taken) { if (!taken) {
return RCL_RET_SUBSCRIPTION_TAKE_FAILED; return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
} }

11
rcl/src/rcl/timer.c
View file

@ -19,8 +19,11 @@ extern "C"
#include "rcl/timer.h" #include "rcl/timer.h"
#include <inttypes.h>
#include "./stdatomic_helper.h" #include "./stdatomic_helper.h"
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#include "rcutils/logging_macros.h"
#include "rcutils/time.h" #include "rcutils/time.h"
typedef struct rcl_timer_impl_t typedef struct rcl_timer_impl_t
@ -53,6 +56,7 @@ rcl_timer_init(
{ {
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator);
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period)
if (timer->impl) { if (timer->impl) {
RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator); RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator);
return RCL_RET_ALREADY_INIT; return RCL_RET_ALREADY_INIT;
@ -91,6 +95,7 @@ rcl_timer_fini(rcl_timer_t * timer)
rcl_ret_t rcl_ret_t
rcl_timer_call(rcl_timer_t * timer) rcl_timer_call(rcl_timer_t * timer)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Calling timer")
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
if (!allocator) { if (!allocator) {
@ -199,6 +204,9 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64
} }
RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT, *allocator);
*old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period); *old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Updated timer period from '%" PRIu64 "ns' to '%" PRIu64 "ns'",
*old_period, new_period)
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -214,6 +222,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer)
rcl_timer_callback_t rcl_timer_callback_t
rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback) rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback)
{ {
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Updating timer callback")
RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL, rcl_get_default_allocator()); RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL, rcl_get_default_allocator());
RCL_CHECK_FOR_NULL_WITH_MSG( RCL_CHECK_FOR_NULL_WITH_MSG(
timer->impl, "timer is invalid", return NULL, rcl_get_default_allocator()); timer->impl, "timer is invalid", return NULL, rcl_get_default_allocator());
@ -228,6 +237,7 @@ rcl_timer_cancel(rcl_timer_t * timer)
RCL_CHECK_FOR_NULL_WITH_MSG( RCL_CHECK_FOR_NULL_WITH_MSG(
timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator()); timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
rcl_atomic_store(&timer->impl->canceled, true); rcl_atomic_store(&timer->impl->canceled, true);
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer canceled")
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -257,6 +267,7 @@ rcl_timer_reset(rcl_timer_t * timer)
} }
rcl_atomic_store(&timer->impl->last_call_time, now); rcl_atomic_store(&timer->impl->last_call_time, now);
rcl_atomic_store(&timer->impl->canceled, false); rcl_atomic_store(&timer->impl->canceled, false);
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset")
return RCL_RET_OK; return RCL_RET_OK;
} }

41
rcl/src/rcl/wait.c
View file

@ -20,12 +20,14 @@ extern "C"
#include "rcl/wait.h" #include "rcl/wait.h"
#include <assert.h> #include <assert.h>
#include <inttypes.h>
#include <stdbool.h> #include <stdbool.h>
#include <string.h> #include <string.h>
#include "./stdatomic_helper.h" #include "./stdatomic_helper.h"
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#include "rcl/time.h" #include "rcl/time.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
#include "rmw/rmw.h" #include "rmw/rmw.h"
@ -113,6 +115,11 @@ rcl_wait_set_init(
size_t number_of_services, size_t number_of_services,
rcl_allocator_t allocator) rcl_allocator_t allocator)
{ {
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing wait set with "
"'%zu' subscriptions, '%zu' guard conditions, '%zu' timers, '%zu' clients, '%zu' services",
number_of_subscriptions, number_of_guard_conditions, number_of_timers, number_of_clients,
number_of_services)
rcl_ret_t fail_ret = RCL_RET_ERROR; rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
@ -575,6 +582,15 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
temporary_timeout_storage.nsec = min_timeout % 1000000000; temporary_timeout_storage.nsec = min_timeout % 1000000000;
timeout_argument = &temporary_timeout_storage; timeout_argument = &temporary_timeout_storage;
} }
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
!timeout_argument, ROS_PACKAGE_NAME, "Waiting without timeout")
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
timeout_argument, ROS_PACKAGE_NAME,
"Waiting with timeout: %" PRIu64 "s + %" PRIu64 "ns",
temporary_timeout_storage.sec, temporary_timeout_storage.nsec)
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Timeout calculated based on next scheduled timer: %s",
is_timer_timeout ? "true" : "false")
// Wait. // Wait.
rmw_ret_t ret = rmw_wait( rmw_ret_t ret = rmw_wait(
@ -584,7 +600,11 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
&wait_set->impl->rmw_clients, &wait_set->impl->rmw_clients,
wait_set->impl->rmw_waitset, wait_set->impl->rmw_waitset,
timeout_argument); timeout_argument);
// Check for ready timers next
// Items that are not ready will have been set to NULL by rmw_wait.
// We now update our handles accordingly.
// Check for ready timers
// and set not ready timers (which includes canceled timers) to NULL. // and set not ready timers (which includes canceled timers) to NULL.
size_t i; size_t i;
for (i = 0; i < wait_set->impl->timer_index; ++i) { for (i = 0; i < wait_set->impl->timer_index; ++i) {
@ -596,6 +616,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set. return ret; // The rcl error state should already be set.
} }
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Timer in wait set is ready")
if (!is_ready) { if (!is_ready) {
wait_set->timers[i] = NULL; wait_set->timers[i] = NULL;
} }
@ -623,25 +644,35 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
} }
// Set corresponding rcl subscription handles NULL. // Set corresponding rcl subscription handles NULL.
for (i = 0; i < wait_set->size_of_subscriptions; ++i) { for (i = 0; i < wait_set->size_of_subscriptions; ++i) {
if (!wait_set->impl->rmw_subscriptions.subscribers[i]) { bool is_ready = wait_set->impl->rmw_subscriptions.subscribers[i] != NULL;
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
is_ready, ROS_PACKAGE_NAME, "Subscription in wait set is ready")
if (!is_ready) {
wait_set->subscriptions[i] = NULL; wait_set->subscriptions[i] = NULL;
} }
} }
// Set corresponding rcl guard_condition handles NULL. // Set corresponding rcl guard_condition handles NULL.
for (i = 0; i < wait_set->size_of_guard_conditions; ++i) { for (i = 0; i < wait_set->size_of_guard_conditions; ++i) {
if (!wait_set->impl->rmw_guard_conditions.guard_conditions[i]) { bool is_ready = wait_set->impl->rmw_guard_conditions.guard_conditions[i] != NULL;
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(
is_ready, ROS_PACKAGE_NAME, "Guard condition in wait set is ready")
if (!is_ready) {
wait_set->guard_conditions[i] = NULL; wait_set->guard_conditions[i] = NULL;
} }
} }
// Set corresponding rcl client handles NULL. // Set corresponding rcl client handles NULL.
for (i = 0; i < wait_set->size_of_clients; ++i) { for (i = 0; i < wait_set->size_of_clients; ++i) {
if (!wait_set->impl->rmw_clients.clients[i]) { bool is_ready = wait_set->impl->rmw_clients.clients[i] != NULL;
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Client in wait set is ready")
if (!is_ready) {
wait_set->clients[i] = NULL; wait_set->clients[i] = NULL;
} }
} }
// Set corresponding rcl service handles NULL. // Set corresponding rcl service handles NULL.
for (i = 0; i < wait_set->size_of_services; ++i) { for (i = 0; i < wait_set->size_of_services; ++i) {
if (!wait_set->impl->rmw_services.services[i]) { bool is_ready = wait_set->impl->rmw_services.services[i] != NULL;
RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Service in wait set is ready")
if (!is_ready) {
wait_set->services[i] = NULL; wait_set->services[i] = NULL;
} }
} }

44
rcl/test/rcl/client_fixture.cpp
View file

@ -41,7 +41,8 @@ wait_for_server_to_be_available(
bool is_ready; bool is_ready;
rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready); rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready);
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR( RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME,
"Error in rcl_service_server_is_available: %s", "Error in rcl_service_server_is_available: %s",
rcl_get_error_string_safe()) rcl_get_error_string_safe())
return false; return false;
@ -63,13 +64,15 @@ wait_for_client_to_be_ready(
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator()); rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator());
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait set init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string_safe())
return false; return false;
} }
auto wait_set_exit = make_scope_exit( auto wait_set_exit = make_scope_exit(
[&wait_set]() { [&wait_set]() {
if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait set fini: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string_safe())
throw std::runtime_error("error while waiting for client"); throw std::runtime_error("error while waiting for client");
} }
}); });
@ -77,11 +80,13 @@ wait_for_client_to_be_ready(
do { do {
++iteration; ++iteration;
if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) { if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait_set_clear_clients: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait_set_clear_clients: %s", rcl_get_error_string_safe())
return false; return false;
} }
if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) { if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait_set_add_client: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string_safe())
return false; return false;
} }
ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
@ -89,7 +94,7 @@ wait_for_client_to_be_ready(
continue; continue;
} }
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string_safe())
return false; return false;
} }
for (size_t i = 0; i < wait_set.size_of_clients; ++i) { for (size_t i = 0; i < wait_set.size_of_clients; ++i) {
@ -106,20 +111,23 @@ int main(int argc, char ** argv)
int main_ret = 0; int main_ret = 0;
{ {
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in rcl init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_t node = rcl_get_zero_initialized_node();
const char * name = "client_fixture_node"; const char * name = "client_fixture_node";
rcl_node_options_t node_options = rcl_node_get_default_options(); rcl_node_options_t node_options = rcl_node_get_default_options();
if (rcl_node_init(&node, name, "", &node_options) != RCL_RET_OK) { if (rcl_node_init(&node, name, "", &node_options) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in node init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto node_exit = make_scope_exit( auto node_exit = make_scope_exit(
[&main_ret, &node]() { [&main_ret, &node]() {
if (rcl_node_fini(&node) != RCL_RET_OK) { if (rcl_node_fini(&node) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in node fini: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
@ -132,21 +140,23 @@ int main(int argc, char ** argv)
rcl_client_options_t client_options = rcl_client_get_default_options(); rcl_client_options_t client_options = rcl_client_get_default_options();
rcl_ret_t ret = rcl_client_init(&client, &node, ts, topic, &client_options); rcl_ret_t ret = rcl_client_init(&client, &node, ts, topic, &client_options);
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in client init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in client init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto client_exit = make_scope_exit( auto client_exit = make_scope_exit(
[&client, &main_ret, &node]() { [&client, &main_ret, &node]() {
if (rcl_client_fini(&client, &node)) { if (rcl_client_fini(&client, &node)) {
RCUTILS_LOG_ERROR("Error in client fini: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in client fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
// Wait until server is available // Wait until server is available
if (!wait_for_server_to_be_available(&node, &client, 1000, 100)) { if (!wait_for_server_to_be_available(&node, &client, 1000, 100)) {
RCUTILS_LOG_ERROR("Server never became available") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Server never became available")
return -1; return -1;
} }
@ -158,12 +168,13 @@ int main(int argc, char ** argv)
int64_t sequence_number; int64_t sequence_number;
if (rcl_send_request(&client, &client_request, &sequence_number)) { if (rcl_send_request(&client, &client_request, &sequence_number)) {
RCUTILS_LOG_ERROR("Error in send request: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in send request: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
if (sequence_number != 1) { if (sequence_number != 1) {
RCUTILS_LOG_ERROR("Got invalid sequence number") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Got invalid sequence number")
return -1; return -1;
} }
@ -174,12 +185,13 @@ int main(int argc, char ** argv)
example_interfaces__srv__AddTwoInts_Response__init(&client_response); example_interfaces__srv__AddTwoInts_Response__init(&client_response);
if (!wait_for_client_to_be_ready(&client, 1000, 100)) { if (!wait_for_client_to_be_ready(&client, 1000, 100)) {
RCUTILS_LOG_ERROR("Client never became ready") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Client never became ready")
return -1; return -1;
} }
rmw_request_id_t header; rmw_request_id_t header;
if (rcl_take_response(&client, &header, &client_response) != RCL_RET_OK) { if (rcl_take_response(&client, &header, &client_response) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in send response: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in send response: %s", rcl_get_error_string_safe())
return -1; return -1;
} }

37
rcl/test/rcl/service_fixture.cpp
View file

@ -37,13 +37,15 @@ wait_for_service_to_be_ready(
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator()); rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator());
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait set init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string_safe())
return false; return false;
} }
auto wait_set_exit = make_scope_exit( auto wait_set_exit = make_scope_exit(
[&wait_set]() { [&wait_set]() {
if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait set fini: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string_safe())
throw std::runtime_error("error waiting for service to be ready"); throw std::runtime_error("error waiting for service to be ready");
} }
}); });
@ -51,11 +53,13 @@ wait_for_service_to_be_ready(
do { do {
++iteration; ++iteration;
if (rcl_wait_set_clear_services(&wait_set) != RCL_RET_OK) { if (rcl_wait_set_clear_services(&wait_set) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait_set_clear_services: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait_set_clear_services: %s", rcl_get_error_string_safe())
return false; return false;
} }
if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) { if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait_set_add_service: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string_safe())
return false; return false;
} }
ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
@ -63,7 +67,7 @@ wait_for_service_to_be_ready(
continue; continue;
} }
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in wait: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string_safe())
return false; return false;
} }
for (size_t i = 0; i < wait_set.size_of_services; ++i) { for (size_t i = 0; i < wait_set.size_of_services; ++i) {
@ -80,20 +84,23 @@ int main(int argc, char ** argv)
int main_ret = 0; int main_ret = 0;
{ {
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in rcl init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_t node = rcl_get_zero_initialized_node();
const char * name = "service_fixture_node"; const char * name = "service_fixture_node";
rcl_node_options_t node_options = rcl_node_get_default_options(); rcl_node_options_t node_options = rcl_node_get_default_options();
if (rcl_node_init(&node, name, "", &node_options) != RCL_RET_OK) { if (rcl_node_init(&node, name, "", &node_options) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in node init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto node_exit = make_scope_exit( auto node_exit = make_scope_exit(
[&main_ret, &node]() { [&main_ret, &node]() {
if (rcl_node_fini(&node) != RCL_RET_OK) { if (rcl_node_fini(&node) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in node fini: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
@ -106,14 +113,16 @@ int main(int argc, char ** argv)
rcl_service_options_t service_options = rcl_service_get_default_options(); rcl_service_options_t service_options = rcl_service_get_default_options();
rcl_ret_t ret = rcl_service_init(&service, &node, ts, topic, &service_options); rcl_ret_t ret = rcl_service_init(&service, &node, ts, topic, &service_options);
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in service init: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in service init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto service_exit = make_scope_exit( auto service_exit = make_scope_exit(
[&main_ret, &service, &node]() { [&main_ret, &service, &node]() {
if (rcl_service_fini(&service, &node)) { if (rcl_service_fini(&service, &node)) {
RCUTILS_LOG_ERROR("Error in service fini: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in service fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
@ -129,7 +138,7 @@ int main(int argc, char ** argv)
// Block until a client request comes in. // Block until a client request comes in.
if (!wait_for_service_to_be_ready(&service, 1000, 100)) { if (!wait_for_service_to_be_ready(&service, 1000, 100)) {
RCUTILS_LOG_ERROR("Service never became ready") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Service never became ready")
return -1; return -1;
} }
@ -143,14 +152,16 @@ int main(int argc, char ** argv)
rmw_request_id_t header; rmw_request_id_t header;
// TODO(jacquelinekay) May have to check for timeout error codes // TODO(jacquelinekay) May have to check for timeout error codes
if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) { if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in take_request: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in take_request: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
// Sum the request and send the response. // Sum the request and send the response.
service_response.sum = service_request.a + service_request.b; service_response.sum = service_request.a + service_request.b;
if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) { if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) {
RCUTILS_LOG_ERROR("Error in send_response: %s", rcl_get_error_string_safe()) RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Error in send_response: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
// Our scope exits should take care of fini for everything // Our scope exits should take care of fini for everything

18
rcl/test/rcl/test_graph.cpp
View file

@ -244,7 +244,7 @@ check_graph_state(
bool expected_in_tnat, bool expected_in_tnat,
size_t number_of_tries) size_t number_of_tries)
{ {
RCUTILS_LOG_INFO( RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
"Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.", "Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
expected_publisher_count, expected_publisher_count,
expected_subscriber_count, expected_subscriber_count,
@ -282,7 +282,7 @@ check_graph_state(
rcl_reset_error(); rcl_reset_error();
} }
RCUTILS_LOG_INFO( RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
" Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.", " Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
i + 1, i + 1,
publisher_count, publisher_count,
@ -294,7 +294,7 @@ check_graph_state(
expected_subscriber_count == subscriber_count && expected_subscriber_count == subscriber_count &&
expected_in_tnat == is_in_tnat) expected_in_tnat == is_in_tnat)
{ {
RCUTILS_LOG_INFO(" state correct!") RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!")
break; break;
} }
// Wait for graph change before trying again. // Wait for graph change before trying again.
@ -307,15 +307,15 @@ check_graph_state(
ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition); ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
RCUTILS_LOG_INFO( RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
" state wrong, waiting up to '%s' nanoseconds for graph changes... ", " state wrong, waiting up to '%s' nanoseconds for graph changes... ",
std::to_string(time_to_sleep.count()).c_str()) std::to_string(time_to_sleep.count()).c_str())
ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); ret = rcl_wait(wait_set_ptr, time_to_sleep.count());
if (ret == RCL_RET_TIMEOUT) { if (ret == RCL_RET_TIMEOUT) {
RCUTILS_LOG_INFO("timeout") RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout")
continue; continue;
} }
RCUTILS_LOG_INFO("change occurred") RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "change occurred")
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
} }
EXPECT_EQ(expected_publisher_count, publisher_count); EXPECT_EQ(expected_publisher_count, publisher_count);
@ -334,7 +334,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
std::string topic_name("/test_graph_query_functions__"); std::string topic_name("/test_graph_query_functions__");
std::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch(); std::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch();
topic_name += std::to_string(now.count()); topic_name += std::to_string(now.count());
RCUTILS_LOG_INFO("Using topic name: %s", topic_name.c_str()) RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Using topic name: %s", topic_name.c_str())
rcl_ret_t ret; rcl_ret_t ret;
const rcl_guard_condition_t * graph_guard_condition = const rcl_guard_condition_t * graph_guard_condition =
rcl_node_get_graph_guard_condition(this->node_ptr); rcl_node_get_graph_guard_condition(this->node_ptr);
@ -468,7 +468,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition); ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
RCUTILS_LOG_INFO( RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
"waiting up to '%s' nanoseconds for graph changes", "waiting up to '%s' nanoseconds for graph changes",
std::to_string(time_to_sleep.count()).c_str()) std::to_string(time_to_sleep.count()).c_str())
ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count()); ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count());
@ -528,7 +528,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition); ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
RCUTILS_LOG_INFO( RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
"waiting up to '%s' nanoseconds for graph changes", "waiting up to '%s' nanoseconds for graph changes",
std::to_string(time_to_sleep.count()).c_str()) std::to_string(time_to_sleep.count()).c_str())
ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count()); ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count());

2
rcl/test/rcl/test_wait.cpp
View file

@ -298,7 +298,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade
ss << "[thread " << test_set.thread_id << "] Timeout (try #" << wake_try_count << ss << "[thread " << test_set.thread_id << "] Timeout (try #" << wake_try_count <<
")"; ")";
// TODO(mikaelarguedas) replace this with stream logging once they exist // TODO(mikaelarguedas) replace this with stream logging once they exist
RCUTILS_LOG_INFO("%s", ss.str().c_str()) RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "%s", ss.str().c_str())
} }
} }
if (!change_detected) { if (!change_detected) {

11
rcl_lifecycle/src/com_interface.c
View file

@ -228,19 +228,20 @@ rcl_lifecycle_com_interface_init(
fail: fail:
if (RCL_RET_OK != rcl_publisher_fini(&com_interface->pub_transition_event, node_handle)) { if (RCL_RET_OK != rcl_publisher_fini(&com_interface->pub_transition_event, node_handle)) {
RCUTILS_LOG_ERROR("Failed to destroy transition_event publisher") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy transition_event publisher")
} }
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_change_state, node_handle)) { if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_change_state, node_handle)) {
RCUTILS_LOG_ERROR("Failed to destroy change_state service") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy change_state service")
} }
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_state, node_handle)) { if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_state, node_handle)) {
RCUTILS_LOG_ERROR("Failed to destroy get_state service") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_state service")
} }
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_states, node_handle)) { if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_states, node_handle)) {
RCUTILS_LOG_ERROR("Failed to destroy get_available_states service") RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to destroy get_available_states service")
} }
if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_transitions, node_handle)) { if (RCL_RET_OK != rcl_service_fini(&com_interface->srv_get_available_transitions, node_handle)) {
RCUTILS_LOG_ERROR("Failed to destroy get_available_transitions service") RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service")
} }
if (topic_name) { if (topic_name) {

12
rcl_lifecycle/src/rcl_lifecycle.c
View file

@ -152,7 +152,7 @@ rcl_lifecycle_is_valid_transition(
} }
} }
RCUTILS_LOG_WARN_NAMED( RCUTILS_LOG_WARN_NAMED(
"rcl_lifecycle", ROS_PACKAGE_NAME,
"No callback transition matching %d found for current state %s", "No callback transition matching %d found for current state %s",
key, state_machine->current_state->label) key, state_machine->current_state->label)
return NULL; return NULL;
@ -169,7 +169,7 @@ rcl_lifecycle_trigger_transition(
// If we have a faulty transition pointer // If we have a faulty transition pointer
if (!transition) { if (!transition) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl_lifecycle", ROS_PACKAGE_NAME,
"No transition found for node %s with key %d", "No transition found for node %s with key %d",
state_machine->current_state->label, key) state_machine->current_state->label, key)
RCL_SET_ERROR_MSG("Transition is not registered.", rcl_get_default_allocator()); RCL_SET_ERROR_MSG("Transition is not registered.", rcl_get_default_allocator());
@ -178,7 +178,7 @@ rcl_lifecycle_trigger_transition(
if (!transition->goal) { if (!transition->goal) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rcl_lifecycle", "No valid goal is set") ROS_PACKAGE_NAME, "No valid goal is set")
} }
state_machine->current_state = transition->goal; state_machine->current_state = transition->goal;
if (publish_notification) { if (publish_notification) {
@ -200,13 +200,15 @@ rcl_print_state_machine(const rcl_lifecycle_state_machine_t * state_machine)
const rcl_lifecycle_transition_map_t * map = &state_machine->transition_map; const rcl_lifecycle_transition_map_t * map = &state_machine->transition_map;
for (size_t i = 0; i < map->states_size; ++i) { for (size_t i = 0; i < map->states_size; ++i) {
RCUTILS_LOG_INFO_NAMED( RCUTILS_LOG_INFO_NAMED(
"rcl_lifecycle", ROS_PACKAGE_NAME,
"Primary State: %s(%u)\n# of valid transitions: %u", "Primary State: %s(%u)\n# of valid transitions: %u",
map->states[i].label, map->states[i].id, map->states[i].label, map->states[i].id,
map->states[i].valid_transition_size map->states[i].valid_transition_size
) )
for (size_t j = 0; j < map->states[i].valid_transition_size; ++j) { for (size_t j = 0; j < map->states[i].valid_transition_size; ++j) {
RCUTILS_LOG_INFO("\tNode %s: Key %d: Transition: %s", RCUTILS_LOG_INFO_NAMED(
ROS_PACKAGE_NAME,
"\tNode %s: Key %d: Transition: %s",
map->states[i].label, map->states[i].label,
map->states[i].valid_transition_keys[j], map->states[i].valid_transition_keys[j],
map->states[i].valid_transitions[j].label) map->states[i].valid_transitions[j].label)

2
rcl_lifecycle/test/test_default_state_machine.cpp
View file

@ -229,7 +229,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) {
*it == lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE || *it == lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE ||
*it == lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN) {continue;} *it == lifecycle_msgs__msg__Transition__TRANSITION_SHUTDOWN) {continue;}
RCUTILS_LOG_INFO("applying key %u", *it) RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "applying key %u", *it)
EXPECT_EQ( EXPECT_EQ(
RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false));
rcl_reset_error(); rcl_reset_error();