diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 1f00bd717..cc64effb0 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -69,7 +69,7 @@ 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) + 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; @@ -93,7 +93,7 @@ rcl_client_init( ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s\n", rcutils_ret, - rcutils_get_error_string_safe()) + rcutils_get_error_string_safe()); } if (ret == RCL_RET_BAD_ALLOC) { return ret; @@ -123,7 +123,7 @@ rcl_client_init( } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name) + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name); const rcl_node_options_t * node_options = rcl_node_get_options(node); if (NULL == node_options) { @@ -176,7 +176,7 @@ rcl_client_init( } // options client->impl->options = *options; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); ret = RCL_RET_OK; goto cleanup; fail: @@ -199,7 +199,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") + 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()); @@ -219,7 +219,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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client finalized"); return result; } @@ -266,7 +266,7 @@ rcl_client_get_rmw_handle(const rcl_client_t * client) 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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client sending service request"); if (!rcl_client_is_valid(client, NULL)) { return RCL_RET_CLIENT_INVALID; } @@ -290,7 +290,7 @@ rcl_take_response( rmw_request_id_t * request_header, void * ros_response) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response"); if (!rcl_client_is_valid(client, NULL)) { return RCL_RET_CLIENT_INVALID; } @@ -307,7 +307,7 @@ rcl_take_response( return RCL_RET_ERROR; } RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Client take response succeeded: %s", taken ? "true" : "false") + 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 ab36c9a7b..74d2bb657 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -155,7 +155,7 @@ rcl_node_init( 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_) + 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; @@ -281,7 +281,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) + 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; @@ -296,7 +296,7 @@ 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") + 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( @@ -359,7 +359,7 @@ rcl_node_init( // error message already set goto fail; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); ret = RCL_RET_OK; goto cleanup; fail: @@ -373,7 +373,7 @@ rcl_node_init( RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, "failed to fini rmw node in error recovery: %s", rmw_get_error_string_safe() - ) + ); } } if (node->impl->graph_guard_condition) { @@ -382,7 +382,7 @@ rcl_node_init( RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, "failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe() - ) + ); } allocator->deallocate(node->impl->graph_guard_condition, allocator->state); } @@ -392,7 +392,7 @@ rcl_node_init( RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, "failed to fini arguments in error recovery: %s", rcl_get_error_string_safe() - ) + ); } } allocator->deallocate(node->impl, allocator->state); @@ -415,7 +415,7 @@ rcl_node_init( rcl_ret_t rcl_node_fini(rcl_node_t * node) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing 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. @@ -444,7 +444,7 @@ rcl_node_fini(rcl_node_t * node) } allocator.deallocate(node->impl, allocator.state); node->impl = NULL; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized") + 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 ef0e5cdf2..346b8fe44 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -72,7 +72,7 @@ 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) + 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(); @@ -92,7 +92,7 @@ rcl_publisher_init( ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s", rcutils_ret, - rcutils_get_error_string_safe()) + rcutils_get_error_string_safe()); } if (ret == RCL_RET_BAD_ALLOC) { return ret; @@ -122,7 +122,7 @@ rcl_publisher_init( } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name) + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name); const rcl_node_options_t * node_options = rcl_node_get_options(node); if (NULL == node_options) { @@ -200,7 +200,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") + 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); @@ -215,7 +215,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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher finalized"); return result; } @@ -233,7 +233,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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher publishing message"); if (!rcl_publisher_is_valid(publisher, NULL)) { return RCL_RET_PUBLISHER_INVALID; } diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 7f12c3d65..8de3b9263 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -139,7 +139,7 @@ rcl_init(int argc, char const * const * argv, rcl_allocator_t allocator) rcl_ret_t rcl_shutdown() { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Shutting down") + 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 64b704b17..672819824 100644 --- a/rcl/src/rcl/rmw_implementation_identifier_check.c +++ b/rcl/src/rcl/rmw_implementation_identifier_check.c @@ -65,14 +65,14 @@ INITIALIZER(initialize) { ROS_PACKAGE_NAME, "Error getting environment variable 'RMW_IMPLEMENTATION': %s", rcl_get_error_string_safe() - ) + ); exit(ret); } if (strlen(expected_rmw_impl_env) > 0) { // Copy the environment variable so it doesn't get over-written by the next getenv call. expected_rmw_impl = rcutils_strdup(expected_rmw_impl_env, allocator); if (!expected_rmw_impl) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "allocation failed") + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "allocation failed"); exit(RCL_RET_BAD_ALLOC); } } @@ -85,14 +85,14 @@ INITIALIZER(initialize) { ROS_PACKAGE_NAME, "Error getting environment variable 'RCL_ASSERT_RMW_ID_MATCHES': %s", rcl_get_error_string_safe() - ) + ); exit(ret); } if (strlen(asserted_rmw_impl_env) > 0) { // Copy the environment variable so it doesn't get over-written by the next getenv call. asserted_rmw_impl = rcutils_strdup(asserted_rmw_impl_env, allocator); if (!asserted_rmw_impl) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "allocation failed") + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "allocation failed"); exit(RCL_RET_BAD_ALLOC); } } @@ -104,7 +104,7 @@ INITIALIZER(initialize) { "Values of RMW_IMPLEMENTATION ('%s') and RCL_ASSERT_RMW_ID_MATCHES ('%s') environment " "variables do not match, exiting with %d.", expected_rmw_impl, asserted_rmw_impl, RCL_RET_ERROR - ) + ); exit(RCL_RET_ERROR); } @@ -132,7 +132,7 @@ INITIALIZER(initialize) { "(expected identifier of '%s'), exiting with %d.", expected_rmw_impl, RCL_RET_ERROR - ) + ); exit(RCL_RET_ERROR); } if (strcmp(actual_rmw_impl_id, expected_rmw_impl) != 0) { @@ -142,7 +142,7 @@ INITIALIZER(initialize) { expected_rmw_impl, actual_rmw_impl_id, RCL_RET_MISMATCHED_RMW_ID - ) + ); exit(RCL_RET_MISMATCHED_RMW_ID); } // Free the memory now that all checking has passed. diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 8167f77e6..71a3477b4 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -66,7 +66,7 @@ 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) + 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; @@ -90,7 +90,7 @@ rcl_service_init( ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s", rcutils_ret, - rcutils_get_error_string_safe()) + rcutils_get_error_string_safe()); } if (ret == RCL_RET_BAD_ALLOC) { return ret; @@ -121,7 +121,7 @@ rcl_service_init( } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name) + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name); const rcl_node_options_t * node_options = rcl_node_get_options(node); if (NULL == node_options) { @@ -165,7 +165,7 @@ rcl_service_init( RCUTILS_LOG_WARN_NAMED( 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.") + "can cause them to receive requests from clients that have since terminated."); } // Fill out implementation struct. // rmw handle (create rmw service) @@ -181,7 +181,7 @@ rcl_service_init( } // options service->impl->options = *options; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); ret = RCL_RET_OK; goto cleanup; fail: @@ -203,7 +203,7 @@ rcl_service_init( rcl_ret_t rcl_service_fini(rcl_service_t * service, rcl_node_t * node) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing service") + 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()); @@ -223,7 +223,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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized"); return result; } @@ -276,7 +276,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") + 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_FOR_NULL_WITH_MSG( options, "Failed to get service options", return RCL_RET_ERROR, rcl_get_default_allocator()); @@ -291,7 +291,7 @@ rcl_take_request( return RCL_RET_ERROR; } RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Service take request succeeded: %s", taken ? "true" : "false") + ROS_PACKAGE_NAME, "Service take request succeeded: %s", taken ? "true" : "false"); if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } @@ -304,7 +304,7 @@ rcl_send_response( rmw_request_id_t * request_header, void * ros_response) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response"); const rcl_service_options_t * options = rcl_service_get_options(service); RCL_CHECK_FOR_NULL_WITH_MSG( options, "Failed to get service options", return RCL_RET_ERROR, rcl_get_default_allocator()); diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h index e821fd285..ffd52084e 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( \ - ROS_PACKAGE_NAME, "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( \ - ROS_PACKAGE_NAME, "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( \ - ROS_PACKAGE_NAME, "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( \ - ROS_PACKAGE_NAME, "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( \ - ROS_PACKAGE_NAME, "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( \ - ROS_PACKAGE_NAME, "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( \ - ROS_PACKAGE_NAME, "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 319419fd5..f66033cdb 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -65,7 +65,7 @@ rcl_subscription_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 subscription for topic name '%s'", topic_name) + 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; @@ -89,7 +89,7 @@ rcl_subscription_init( ROS_PACKAGE_NAME, "failed to fini string_map (%d) during error handling: %s", rcutils_ret, - rcutils_get_error_string_safe()) + rcutils_get_error_string_safe()); } if (ret == RCL_RET_BAD_ALLOC) { return ret; @@ -119,7 +119,7 @@ rcl_subscription_init( } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name) + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name); const rcl_node_options_t * node_options = rcl_node_get_options(node); if (NULL == node_options) { @@ -174,7 +174,7 @@ rcl_subscription_init( } // options subscription->impl->options = *options; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; goto cleanup; fail: @@ -196,7 +196,7 @@ rcl_subscription_init( rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing subscription") + 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()); @@ -217,7 +217,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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription finalized"); return result; } @@ -240,7 +240,7 @@ rcl_take( void * ros_message, rmw_message_info_t * message_info) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message"); rcl_allocator_t error_allocator = rcl_get_default_allocator(); if (!rcl_subscription_is_valid(subscription, &error_allocator)) { return RCL_RET_SUBSCRIPTION_INVALID; // error message already set @@ -259,7 +259,7 @@ rcl_take( return RCL_RET_ERROR; } RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false") + ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false"); if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } @@ -272,7 +272,7 @@ rcl_take_serialized_message( rcl_serialized_message_t * serialized_message, rmw_message_info_t * message_info) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message"); rcl_allocator_t error_allocator = rcl_get_default_allocator(); if (!rcl_subscription_is_valid(subscription, &error_allocator)) { return RCL_RET_SUBSCRIPTION_INVALID; // error message already set @@ -294,7 +294,7 @@ rcl_take_serialized_message( return RCL_RET_ERROR; } RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false") + ROS_PACKAGE_NAME, "Subscription serialized 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 d2fd1eb27..be1e5042d 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -134,7 +134,8 @@ rcl_timer_init( RCL_SET_ERROR_MSG("timer period must be non-negative", allocator); return RCL_RET_INVALID_ARGUMENT; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period) + 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; @@ -231,7 +232,7 @@ rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock) rcl_ret_t rcl_timer_call(rcl_timer_t * timer) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Calling 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) { @@ -366,7 +367,7 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, int64_t new_period, int64_t *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) + *old_period, new_period); return RCL_RET_OK; } @@ -382,7 +383,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") + 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()); @@ -397,7 +398,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") + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer canceled"); return RCL_RET_OK; } @@ -428,7 +429,7 @@ rcl_timer_reset(rcl_timer_t * timer) int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); rcl_atomic_store(&timer->impl->next_call_time, now + period); rcl_atomic_store(&timer->impl->canceled, false); - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset") + 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 abc1f60b8..fbc08b7e6 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -104,7 +104,7 @@ rcl_wait_set_init( 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) + number_of_services); rcl_ret_t fail_ret = RCL_RET_ERROR; RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); @@ -534,14 +534,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) timeout_argument = &temporary_timeout_storage; } RCUTILS_LOG_DEBUG_EXPRESSION_NAMED( - !timeout_argument, ROS_PACKAGE_NAME, "Waiting without timeout") + !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) + 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") + is_timer_timeout ? "true" : "false"); // Wait. rmw_ret_t ret = rmw_wait( @@ -567,7 +567,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") + RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Timer in wait set is ready"); if (!is_ready) { wait_set->timers[i] = NULL; } @@ -581,7 +581,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) for (i = 0; i < wait_set->size_of_subscriptions; ++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") + is_ready, ROS_PACKAGE_NAME, "Subscription in wait set is ready"); if (!is_ready) { wait_set->subscriptions[i] = NULL; } @@ -590,7 +590,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) for (i = 0; i < wait_set->size_of_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") + is_ready, ROS_PACKAGE_NAME, "Guard condition in wait set is ready"); if (!is_ready) { wait_set->guard_conditions[i] = NULL; } @@ -598,7 +598,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Set corresponding rcl client handles NULL. for (i = 0; i < wait_set->size_of_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") + RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Client in wait set is ready"); if (!is_ready) { wait_set->clients[i] = NULL; } @@ -606,7 +606,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Set corresponding rcl service handles NULL. for (i = 0; i < wait_set->size_of_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") + 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 3bbe90e9e..6d020ecd1 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -43,7 +43,7 @@ wait_for_server_to_be_available( RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, "Error in rcl_service_server_is_available: %s", - rcl_get_error_string_safe()) + rcl_get_error_string_safe()); return false; } if (is_ready) { @@ -64,13 +64,13 @@ wait_for_client_to_be_ready( 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_NAMED( - ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string_safe()); return false; } OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string_safe()); throw std::runtime_error("error while waiting for client"); } }); @@ -79,12 +79,12 @@ wait_for_client_to_be_ready( ++iteration; if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe()); return false; } if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string_safe()) + 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)); @@ -92,7 +92,7 @@ wait_for_client_to_be_ready( continue; } if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "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) { @@ -110,7 +110,7 @@ int main(int argc, char ** argv) { if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string_safe()); return -1; } rcl_node_t node = rcl_get_zero_initialized_node(); @@ -118,13 +118,13 @@ int main(int argc, char ** argv) 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_NAMED( - ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string_safe()); return -1; } OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ if (rcl_node_fini(&node) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string_safe()); main_ret = -1; } }); @@ -138,21 +138,21 @@ int main(int argc, char ** argv) rcl_ret_t ret = rcl_client_init(&client, &node, ts, service_name, &client_options); if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in client init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in client init: %s", rcl_get_error_string_safe()); return -1; } OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ if (rcl_client_fini(&client, &node)) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in client fini: %s", rcl_get_error_string_safe()) + 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_NAMED(ROS_PACKAGE_NAME, "Server never became available") + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Server never became available"); return -1; } @@ -168,12 +168,12 @@ int main(int argc, char ** argv) if (rcl_send_request(&client, &client_request, &sequence_number)) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in send request: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in send request: %s", rcl_get_error_string_safe()); return -1; } if (sequence_number != 1) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Got invalid sequence number") + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Got invalid sequence number"); return -1; } @@ -187,13 +187,13 @@ int main(int argc, char ** argv) test_msgs__srv__Primitives_Response__init(&client_response); if (!wait_for_client_to_be_ready(&client, 1000, 100)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "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_NAMED( - ROS_PACKAGE_NAME, "Error in send response: %s", rcl_get_error_string_safe()) + 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 61b129a03..11e5d0f45 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -37,13 +37,13 @@ wait_for_service_to_be_ready( 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_NAMED( - ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string_safe()); return false; } OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string_safe()) + 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"); } }); @@ -52,12 +52,12 @@ wait_for_service_to_be_ready( ++iteration; if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe()); return false; } if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string_safe()) + 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)); @@ -65,7 +65,7 @@ wait_for_service_to_be_ready( continue; } if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "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) { @@ -83,7 +83,7 @@ int main(int argc, char ** argv) { if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string_safe()); return -1; } rcl_node_t node = rcl_get_zero_initialized_node(); @@ -91,13 +91,13 @@ int main(int argc, char ** argv) 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_NAMED( - ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string_safe()); return -1; } OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ if (rcl_node_fini(&node) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string_safe()); main_ret = -1; } }); @@ -111,14 +111,14 @@ int main(int argc, char ** argv) rcl_ret_t ret = rcl_service_init(&service, &node, ts, service_name, &service_options); if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in service init: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in service init: %s", rcl_get_error_string_safe()); return -1; } OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ if (rcl_service_fini(&service, &node)) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in service fini: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in service fini: %s", rcl_get_error_string_safe()); main_ret = -1; } }); @@ -136,7 +136,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_NAMED(ROS_PACKAGE_NAME, "Service never became ready") + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Service never became ready"); return -1; } @@ -153,7 +153,7 @@ int main(int argc, char ** argv) // TODO(jacquelinekay) May have to check for timeout error codes if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in take_request: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in take_request: %s", rcl_get_error_string_safe()); return -1; } @@ -161,7 +161,7 @@ int main(int argc, char ** argv) service_response.uint64_value = service_request.uint8_value + service_request.uint32_value; if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in send_response: %s", rcl_get_error_string_safe()) + 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 41e2fa7b9..c0684cc70 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -232,7 +232,7 @@ check_graph_state( expected_publisher_count, expected_subscriber_count, expected_in_tnat ? "" : " not" - ) + ); size_t publisher_count = 0; size_t subscriber_count = 0; bool is_in_tnat = false; @@ -271,13 +271,13 @@ check_graph_state( publisher_count, subscriber_count, is_in_tnat ? "" : " not" - ) + ); if ( expected_publisher_count == publisher_count && expected_subscriber_count == subscriber_count && expected_in_tnat == is_in_tnat) { - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!") + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!"); break; } // Wait for graph change before trying again. @@ -292,13 +292,13 @@ check_graph_state( std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); 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()) + 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_NAMED(ROS_PACKAGE_NAME, "timeout") + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout"); continue; } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "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); @@ -316,7 +316,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_NAMED(ROS_PACKAGE_NAME, "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); @@ -451,7 +451,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "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()); if (ret == RCL_RET_TIMEOUT) { continue; @@ -508,7 +508,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "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()); if (ret == RCL_RET_TIMEOUT) { if (!is_connext) { diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index d70dda2f0..c77ab7f14 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -313,7 +313,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_NAMED(ROS_PACKAGE_NAME, "%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 d329386e2..bdc330684 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/rcl_lifecycle/src/com_interface.c @@ -145,20 +145,20 @@ rcl_lifecycle_com_interface_init( fail: if (RCL_RET_OK != rcl_publisher_fini(&com_interface->pub_transition_event, node_handle)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "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_NAMED(ROS_PACKAGE_NAME, "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_NAMED(ROS_PACKAGE_NAME, "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_NAMED(ROS_PACKAGE_NAME, "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_NAMED( - ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service") + ROS_PACKAGE_NAME, "Failed to destroy get_available_transitions service"); } return RCL_RET_ERROR; diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index cff756f51..7b23cc184 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -297,7 +297,7 @@ rcl_lifecycle_is_valid_transition( RCUTILS_LOG_WARN_NAMED( ROS_PACKAGE_NAME, "No callback transition matching %d found for current state %s", - key, state_machine->current_state->label) + key, state_machine->current_state->label); return NULL; } @@ -315,14 +315,14 @@ rcl_lifecycle_trigger_transition( RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, "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()); return RCL_RET_ERROR; } if (!transition->goal) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "No valid goal is set") + ROS_PACKAGE_NAME, "No valid goal is set"); return RCL_RET_ERROR; } state_machine->current_state = transition->goal; @@ -349,14 +349,14 @@ rcl_print_state_machine(const rcl_lifecycle_state_machine_t * state_machine) "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_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) + 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 37b743be2..a3a1f4cb0 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_NAMED(ROS_PACKAGE_NAME, "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();