diff --git a/rcl/include/rcl/expand_topic_name.h b/rcl/include/rcl/expand_topic_name.h index 20ac860..48f6edb 100644 --- a/rcl/include/rcl/expand_topic_name.h +++ b/rcl/include/rcl/expand_topic_name.h @@ -64,7 +64,7 @@ extern "C" * if (ret != RCL_RET_OK) { * // ... error handling * } 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: * allocator.deallocate(expanded_topic_name, allocator.state); * } diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 1ffd254..26c2ef4 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -67,6 +67,8 @@ rcl_client_init( } RCL_CHECK_ARGUMENT_FOR_NULL(type_support, 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) { RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized", *allocator); return RCL_RET_ALREADY_INIT; @@ -87,7 +89,7 @@ rcl_client_init( rcutils_ret = rcutils_string_map_fini(&substitutions_map); if (rcutils_ret != RCUTILS_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s\n", rcutils_ret, rcutils_get_error_string_safe()) @@ -120,6 +122,7 @@ rcl_client_init( return RCL_RET_ERROR; } } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name) // Validate the expanded service name. int validation_result; rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL); @@ -151,6 +154,7 @@ rcl_client_init( } // options client->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized") return RCL_RET_OK; fail: if (client->impl) { @@ -163,6 +167,7 @@ rcl_ret_t rcl_client_fini(rcl_client_t * client, rcl_node_t * node) { (void)node; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing client") 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(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); } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client finalized") return result; } @@ -232,6 +238,7 @@ RCL_WARN_UNUSED rcl_ret_t 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)) { return RCL_RET_CLIENT_INVALID; } @@ -257,6 +264,7 @@ rcl_take_response( rmw_request_id_t * request_header, void * ros_response) { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response") if (!rcl_client_is_valid(client)) { 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); return RCL_RET_ERROR; } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Client take response succeeded: %s", taken ? "true" : "false") if (!taken) { return RCL_RET_CLIENT_TAKE_FAILED; } diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 952afc9..a5c3376 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -108,6 +108,8 @@ rcl_node_init( 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(node, RCL_RET_INVALID_ARGUMENT, *allocator); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing node '%s' in namespace '%s'", name, namespace_) if (node->impl) { RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized", *allocator); return RCL_RET_ALREADY_INIT; @@ -215,6 +217,7 @@ rcl_node_init( } // actual 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_enforce_security = NULL; @@ -230,6 +233,8 @@ rcl_node_init( } 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)) { RCL_SET_ERROR_MSG( @@ -301,6 +306,7 @@ rcl_node_init( // error message already set goto fail; } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized") return RCL_RET_OK; fail: if (node->impl) { @@ -308,7 +314,7 @@ fail: ret = rmw_destroy_node(node->impl->rmw_node_handle); if (ret != RMW_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "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); if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe() ) } @@ -337,6 +343,7 @@ fail: rcl_ret_t 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()); if (!node->impl) { // 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 allocator.deallocate(node->impl, allocator.state); node->impl = NULL; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized") return result; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index d30ad2e..973147d 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -70,6 +70,8 @@ rcl_publisher_init( } RCL_CHECK_ARGUMENT_FOR_NULL(type_support, 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. rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version 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); if (rcutils_ret != RCUTILS_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s", rcutils_ret, rcutils_get_error_string_safe()) @@ -119,6 +121,7 @@ rcl_publisher_init( return RCL_RET_ERROR; } } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name) // Validate the expanded topic name. int validation_result; 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); // options publisher->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized") return RCL_RET_OK; fail: 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)) { return RCL_RET_NODE_INVALID; } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing publisher") if (publisher->impl) { rcl_allocator_t allocator = publisher->impl->options.allocator; 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); } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher finalized") return result; } @@ -196,6 +202,7 @@ rcl_publisher_get_default_options() rcl_ret_t 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)) { return RCL_RET_PUBLISHER_INVALID; } diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index c91717d..c0cb873 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -23,6 +23,7 @@ extern "C" #include "./stdatomic_helper.h" #include "rcl/error_handling.h" +#include "rcutils/logging_macros.h" #include "rmw/error_handling.h" static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false); @@ -116,6 +117,7 @@ fail: rcl_ret_t rcl_shutdown() { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Shutting down") if (!rcl_ok()) { // 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()); diff --git a/rcl/src/rcl/rmw_implementation_identifier_check.c b/rcl/src/rcl/rmw_implementation_identifier_check.c index f7e7a18..49479c4 100644 --- a/rcl/src/rcl/rmw_implementation_identifier_check.c +++ b/rcl/src/rcl/rmw_implementation_identifier_check.c @@ -57,7 +57,7 @@ INITIALIZER(initialize) { rcl_ret_t ret = rcl_impl_getenv("RCL_ASSERT_RMW_ID_MATCHES", &expected); if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s", 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 (strlen(expected) > 0 && strcmp(rmw_get_implementation_identifier(), expected) != 0) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.", expected, rmw_get_implementation_identifier(), diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 9b58529..d8f33f0 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -64,6 +64,8 @@ rcl_service_init( } RCL_CHECK_ARGUMENT_FOR_NULL(type_support, 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) { RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized", *allocator); return RCL_RET_ALREADY_INIT; @@ -84,7 +86,7 @@ rcl_service_init( rcutils_ret = rcutils_string_map_fini(&substitutions_map); if (rcutils_ret != RCUTILS_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s", rcutils_ret, rcutils_get_error_string_safe()) @@ -117,6 +119,7 @@ rcl_service_init( return RCL_RET_ERROR; } } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name) // Validate the expanded service name. int validation_result; 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) { RCUTILS_LOG_WARN_NAMED( - "rcl", + ROS_PACKAGE_NAME, "Warning: Setting QoS durability to 'transient local' for service servers " "can cause them to receive requests from clients that have since terminated.") } @@ -155,6 +158,7 @@ rcl_service_init( } // options service->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized") return RCL_RET_OK; fail: if (service->impl) { @@ -166,6 +170,7 @@ fail: rcl_ret_t 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_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()); @@ -185,6 +190,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) } allocator.deallocate(service->impl, allocator.state); } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized") return result; } @@ -239,6 +245,7 @@ rcl_take_request( rmw_request_id_t * request_header, 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); 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); @@ -250,6 +257,8 @@ rcl_take_request( RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator); return RCL_RET_ERROR; } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Service take request succeeded: %s", taken ? "true" : "false") if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } @@ -262,6 +271,7 @@ rcl_send_response( rmw_request_id_t * request_header, void * ros_response) { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response") 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(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); return RCL_RET_ERROR; } - return RCL_RET_OK; } diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h index 85ee9ba..e821fd2 100644 --- a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h +++ b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h @@ -209,7 +209,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ 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); \ break; \ } \ @@ -238,7 +238,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ RCUTILS_LOG_ERROR_NAMED( \ - "rcl", "Unsupported integer type in atomic_exchange_strong") \ + ROS_PACKAGE_NAME, "Unsupported integer type in atomic_exchange_strong") \ exit(-1); \ break; \ } \ @@ -264,7 +264,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ RCUTILS_LOG_ERROR_NAMED( \ - "rcl", "Unsupported integer type in atomic_fetch_add") \ + ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_add") \ exit(-1); \ break; \ } \ @@ -290,7 +290,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ RCUTILS_LOG_ERROR_NAMED( \ - "rcl", "Unsupported integer type in atomic_fetch_and") \ + ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_and") \ exit(-1); \ break; \ } \ @@ -316,7 +316,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ RCUTILS_LOG_ERROR_NAMED( \ - "rcl", "Unsupported integer type in atomic_fetch_or") \ + ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_or") \ exit(-1); \ break; \ } \ @@ -345,7 +345,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ RCUTILS_LOG_ERROR_NAMED( \ - "rcl", "Unsupported integer type in atomic_fetch_xor") \ + ROS_PACKAGE_NAME, "Unsupported integer type in atomic_fetch_xor") \ exit(-1); \ break; \ } \ @@ -371,7 +371,7 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; break; \ default: \ RCUTILS_LOG_ERROR_NAMED( \ - "rcl", "Unsupported integer type in atomic_load") \ + ROS_PACKAGE_NAME, "Unsupported integer type in atomic_load") \ exit(-1); \ break; \ } \ diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index eba950b..6f40c79 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -63,6 +63,8 @@ rcl_subscription_init( 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(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) { RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized", *allocator); return RCL_RET_ALREADY_INIT; @@ -83,7 +85,7 @@ rcl_subscription_init( rcutils_ret = rcutils_string_map_fini(&substitutions_map); if (rcutils_ret != RCUTILS_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rcl", + ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s", rcutils_ret, rcutils_get_error_string_safe()) @@ -116,6 +118,7 @@ rcl_subscription_init( return RCL_RET_ERROR; } } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name) // Validate the expanded topic name. int validation_result; rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL); @@ -148,6 +151,7 @@ rcl_subscription_init( } // options subscription->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized") return RCL_RET_OK; fail: if (subscription->impl) { @@ -159,6 +163,7 @@ fail: rcl_ret_t 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_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()); @@ -179,6 +184,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) } allocator.deallocate(subscription->impl, allocator.state); } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription finalized") return result; } @@ -201,6 +207,7 @@ rcl_take( void * ros_message, 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()); const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription); if (!options) { @@ -224,6 +231,8 @@ rcl_take( RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator); return RCL_RET_ERROR; } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false") if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 936890c..b79edb6 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -19,8 +19,11 @@ extern "C" #include "rcl/timer.h" +#include + #include "./stdatomic_helper.h" #include "rcl/error_handling.h" +#include "rcutils/logging_macros.h" #include "rcutils/time.h" 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_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) { RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator); return RCL_RET_ALREADY_INIT; @@ -91,6 +95,7 @@ rcl_timer_fini(rcl_timer_t * timer) rcl_ret_t 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()); const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); 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); *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; } @@ -214,6 +222,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer) rcl_timer_callback_t 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_FOR_NULL_WITH_MSG( 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( timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator()); rcl_atomic_store(&timer->impl->canceled, true); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer canceled") 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->canceled, false); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset") return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a6c562c..f51f48e 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -20,12 +20,14 @@ extern "C" #include "rcl/wait.h" #include +#include #include #include #include "./stdatomic_helper.h" #include "rcl/error_handling.h" #include "rcl/time.h" +#include "rcutils/logging_macros.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -113,6 +115,11 @@ rcl_wait_set_init( size_t number_of_services, 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_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; 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. 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_waitset, 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. size_t 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) { 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) { 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. 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; } } // Set corresponding rcl guard_condition handles NULL. 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; } } // Set corresponding rcl client handles NULL. 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; } } // Set corresponding rcl service handles NULL. 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; } } diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index 999663f..4cfbef7 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -41,7 +41,8 @@ wait_for_server_to_be_available( bool is_ready; rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready); if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl_service_server_is_available: %s", rcl_get_error_string_safe()) 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_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator()); 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; } auto wait_set_exit = make_scope_exit( [&wait_set]() { 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"); } }); @@ -77,11 +80,13 @@ wait_for_client_to_be_ready( do { ++iteration; 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; } 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; } ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); @@ -89,7 +94,7 @@ wait_for_client_to_be_ready( continue; } 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; } 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; { 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; } rcl_node_t node = rcl_get_zero_initialized_node(); const char * name = "client_fixture_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); 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; } auto node_exit = make_scope_exit( [&main_ret, &node]() { 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; } }); @@ -132,21 +140,23 @@ int main(int argc, char ** argv) rcl_client_options_t client_options = rcl_client_get_default_options(); rcl_ret_t ret = rcl_client_init(&client, &node, ts, topic, &client_options); 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; } auto client_exit = make_scope_exit( [&client, &main_ret, &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; } }); // Wait until server is available 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; } @@ -158,12 +168,13 @@ int main(int argc, char ** argv) int64_t 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; } if (sequence_number != 1) { - RCUTILS_LOG_ERROR("Got invalid sequence number") + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Got invalid sequence number") return -1; } @@ -174,12 +185,13 @@ int main(int argc, char ** argv) example_interfaces__srv__AddTwoInts_Response__init(&client_response); 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; } rmw_request_id_t header; 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; } diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp index 8512964..ae752c8 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -37,13 +37,15 @@ wait_for_service_to_be_ready( 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()); 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; } auto wait_set_exit = make_scope_exit( [&wait_set]() { 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"); } }); @@ -51,11 +53,13 @@ wait_for_service_to_be_ready( do { ++iteration; 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; } 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; } ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); @@ -63,7 +67,7 @@ wait_for_service_to_be_ready( continue; } 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; } 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; { 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; } rcl_node_t node = rcl_get_zero_initialized_node(); const char * name = "service_fixture_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); 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; } auto node_exit = make_scope_exit( [&main_ret, &node]() { 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; } }); @@ -106,14 +113,16 @@ int main(int argc, char ** argv) rcl_service_options_t service_options = rcl_service_get_default_options(); rcl_ret_t ret = rcl_service_init(&service, &node, ts, topic, &service_options); 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; } auto service_exit = make_scope_exit( [&main_ret, &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; } }); @@ -129,7 +138,7 @@ int main(int argc, char ** argv) // Block until a client request comes in. 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; } @@ -143,14 +152,16 @@ int main(int argc, char ** argv) rmw_request_id_t header; // TODO(jacquelinekay) May have to check for timeout error codes 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; } // Sum the request and send the response. service_response.sum = service_request.a + service_request.b; 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; } // Our scope exits should take care of fini for everything diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 31ca8fd..0d75ef3 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -244,7 +244,7 @@ check_graph_state( bool expected_in_tnat, 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.", expected_publisher_count, expected_subscriber_count, @@ -282,7 +282,7 @@ check_graph_state( 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.", i + 1, publisher_count, @@ -294,7 +294,7 @@ check_graph_state( expected_subscriber_count == subscriber_count && expected_in_tnat == is_in_tnat) { - RCUTILS_LOG_INFO(" state correct!") + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!") break; } // 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); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); 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... ", std::to_string(time_to_sleep.count()).c_str()) ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); if (ret == RCL_RET_TIMEOUT) { - RCUTILS_LOG_INFO("timeout") + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout") 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(); } 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::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch(); 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; const rcl_guard_condition_t * graph_guard_condition = 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); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); 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", std::to_string(time_to_sleep.count()).c_str()) 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(); 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(); - RCUTILS_LOG_INFO( + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "waiting up to '%s' nanoseconds for graph changes", std::to_string(time_to_sleep.count()).c_str()) ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count()); diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index f853df9..99d8be4 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -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 << ")"; // 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) { diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index 6b21d12..f118b4a 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -228,19 +228,20 @@ rcl_lifecycle_com_interface_init( fail: 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)) { - 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)) { - 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)) { - 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)) { - 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) { diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index afeec51..9a8f66d 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -152,7 +152,7 @@ rcl_lifecycle_is_valid_transition( } } RCUTILS_LOG_WARN_NAMED( - "rcl_lifecycle", + ROS_PACKAGE_NAME, "No callback transition matching %d found for current state %s", key, state_machine->current_state->label) return NULL; @@ -169,7 +169,7 @@ rcl_lifecycle_trigger_transition( // If we have a faulty transition pointer if (!transition) { RCUTILS_LOG_ERROR_NAMED( - "rcl_lifecycle", + ROS_PACKAGE_NAME, "No transition found for node %s with key %d", state_machine->current_state->label, key) RCL_SET_ERROR_MSG("Transition is not registered.", rcl_get_default_allocator()); @@ -178,7 +178,7 @@ rcl_lifecycle_trigger_transition( if (!transition->goal) { 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; 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; for (size_t i = 0; i < map->states_size; ++i) { RCUTILS_LOG_INFO_NAMED( - "rcl_lifecycle", + ROS_PACKAGE_NAME, "Primary State: %s(%u)\n# of valid transitions: %u", map->states[i].label, map->states[i].id, map->states[i].valid_transition_size ) 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].valid_transition_keys[j], map->states[i].valid_transitions[j].label) diff --git a/rcl_lifecycle/test/test_default_state_machine.cpp b/rcl_lifecycle/test/test_default_state_machine.cpp index a8e0d8f..37b743b 100644 --- a/rcl_lifecycle/test/test_default_state_machine.cpp +++ b/rcl_lifecycle/test/test_default_state_machine.cpp @@ -229,7 +229,7 @@ TEST_F(TestDefaultStateMachine, wrong_default_sequence) { *it == lifecycle_msgs__msg__Transition__TRANSITION_ACTIVATE || *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( RCL_RET_ERROR, rcl_lifecycle_trigger_transition(&state_machine, *it, false)); rcl_reset_error();