use macros

This commit is contained in:
Mikael Arguedas 2017-08-09 12:02:06 -07:00
parent 3cff9020d1
commit 835a90761c
13 changed files with 133 additions and 94 deletions

View file

@ -64,7 +64,7 @@ extern "C"
* if (ret != RCL_RET_OK) { * if (ret != RCL_RET_OK) {
* // ... error handling * // ... error handling
* } else { * } else {
* printf("Expanded topic name: %s\n", expanded_topic_name); * RCUTILS_LOG_INFO("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);
* } * }

View file

@ -23,6 +23,7 @@ extern "C"
#include <string.h> #include <string.h>
#include "rcl/expand_topic_name.h" #include "rcl/expand_topic_name.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
#include "rmw/rmw.h" #include "rmw/rmw.h"
#include "rmw/validate_full_topic_name.h" #include "rmw/validate_full_topic_name.h"
@ -91,11 +92,11 @@ rcl_client_init(
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " "rcl",
"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())
} }
if (ret == RCL_RET_BAD_ALLOC) { if (ret == RCL_RET_BAD_ALLOC) {
return ret; return ret;

View file

@ -27,6 +27,7 @@ extern "C"
#include "rcl/rcl.h" #include "rcl/rcl.h"
#include "rcutils/filesystem.h" #include "rcutils/filesystem.h"
#include "rcutils/get_env.h" #include "rcutils/get_env.h"
#include "rcutils/logging_macros.h"
#include "rcutils/macros.h" #include "rcutils/macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
#include "rmw/node_security_options.h" #include "rmw/node_security_options.h"
@ -295,17 +296,19 @@ fail:
if (node->impl->rmw_node_handle) { if (node->impl->rmw_node_handle) {
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"failed to fini rmw node in error recovery: %s\n", rmw_get_error_string_safe() "rcl",
); "failed to fini rmw node in error recovery: %s", rmw_get_error_string_safe()
)
} }
} }
if (node->impl->graph_guard_condition) { if (node->impl->graph_guard_condition) {
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"failed to fini guard condition in error recovery: %s\n", rcl_get_error_string_safe() "rcl",
); "failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe()
)
} }
allocator->deallocate(node->impl->graph_guard_condition, allocator->state); allocator->deallocate(node->impl->graph_guard_condition, allocator->state);
} }

View file

@ -24,6 +24,7 @@ extern "C"
#include "./common.h" #include "./common.h"
#include "rcl/expand_topic_name.h" #include "rcl/expand_topic_name.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
#include "rmw/rmw.h" #include "rmw/rmw.h"
#include "rmw/validate_full_topic_name.h" #include "rmw/validate_full_topic_name.h"
@ -89,11 +90,11 @@ rcl_publisher_init(
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " "rcl",
"failed to fini string_map (%d) during error handling: %s\n", "failed to fini string_map (%d) during error handling: %s",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()); rcutils_get_error_string_safe())
} }
if (ret == RCL_RET_BAD_ALLOC) { if (ret == RCL_RET_BAD_ALLOC) {
return ret; return ret;

View file

@ -21,6 +21,7 @@ extern "C"
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <rcutils/logging_macros.h>
#include <rmw/rmw.h> #include <rmw/rmw.h>
#include "rcl/types.h" #include "rcl/types.h"
@ -54,20 +55,22 @@ INITIALIZER(initialize) {
const char * expected = NULL; const char * expected = NULL;
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s\n", "rcl",
"Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s",
rcl_get_error_string_safe() rcl_get_error_string_safe()
); )
exit(ret); exit(ret);
} }
// 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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.\n", "rcl",
"Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.",
expected, expected,
rmw_get_implementation_identifier(), rmw_get_implementation_identifier(),
RCL_RET_MISMATCHED_RMW_ID RCL_RET_MISMATCHED_RMW_ID
); )
exit(RCL_RET_MISMATCHED_RMW_ID); exit(RCL_RET_MISMATCHED_RMW_ID);
} }
} }

View file

@ -24,6 +24,7 @@ extern "C"
#include "./common.h" #include "./common.h"
#include "rcl/expand_topic_name.h" #include "rcl/expand_topic_name.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
#include "rmw/rmw.h" #include "rmw/rmw.h"
#include "rmw/validate_full_topic_name.h" #include "rmw/validate_full_topic_name.h"
@ -88,11 +89,11 @@ rcl_service_init(
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " "rcl",
"failed to fini string_map (%d) during error handling: %s\n", "failed to fini string_map (%d) during error handling: %s",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()); rcutils_get_error_string_safe())
} }
if (ret == RCL_RET_BAD_ALLOC) { if (ret == RCL_RET_BAD_ALLOC) {
return ret; return ret;
@ -140,8 +141,10 @@ rcl_service_init(
service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator); service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) { if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
fprintf(stderr, "Warning: Setting QoS durability to 'transient local' for service servers " RCUTILS_LOG_WARN_NAMED(
"can cause them to receive requests from clients that have since terminated.\n"); "rcl",
"Warning: Setting QoS durability to 'transient local' for service servers "
"can cause them to receive requests from clients that have since terminated.")
} }
// Fill out implementation struct. // Fill out implementation struct.
// rmw handle (create rmw service) // rmw handle (create rmw service)

View file

@ -69,6 +69,8 @@
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <rcutils/logging_macros.h>
// In MSVC, correct alignment of each type is already ensured. // In MSVC, correct alignment of each type is already ensured.
#define _Atomic(T) struct { T __val; } #define _Atomic(T) struct { T __val; }
@ -206,7 +208,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedCompareExchange8((char *) object, desired, *expected); \ out = _InterlockedCompareExchange8((char *) object, desired, *expected); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_compare_exchange_strong"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_compare_exchange_strong") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -234,7 +237,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedExchange8((char *) object, desired); \ out = _InterlockedExchange8((char *) object, desired); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_exchange_strong"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_exchange_strong") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -259,7 +263,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedExchangeAdd8((char *) object, operand); \ out = _InterlockedExchangeAdd8((char *) object, operand); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_fetch_add"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_add") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -284,7 +289,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedAnd8((char *) object, operand); \ out = _InterlockedAnd8((char *) object, operand); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_fetch_and"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_and") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -309,7 +315,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedOr8((char *) object, operand); \ out = _InterlockedOr8((char *) object, operand); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_fetch_or"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_or") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -337,7 +344,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedXor8((char *) object, operand); \ out = _InterlockedXor8((char *) object, operand); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_fetch_xor"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_fetch_xor") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \
@ -362,7 +370,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
out = _InterlockedExchangeAdd8((char *) object, 0); \ out = _InterlockedExchangeAdd8((char *) object, 0); \
break; \ break; \
default: \ default: \
fprintf(stderr, "Unsupported integer type in atomic_load"); \ RCUTILS_LOG_ERROR_NAMED( \
"rcl", "Unsupported integer type in atomic_load") \
exit(-1); \ exit(-1); \
break; \ break; \
} \ } \

View file

@ -23,6 +23,7 @@ extern "C"
#include "./common.h" #include "./common.h"
#include "rcl/expand_topic_name.h" #include "rcl/expand_topic_name.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
#include "rmw/rmw.h" #include "rmw/rmw.h"
#include "rmw/validate_full_topic_name.h" #include "rmw/validate_full_topic_name.h"
@ -83,11 +84,11 @@ rcl_subscription_init(
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
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) {
fprintf(stderr, RCUTILS_LOG_ERROR_NAMED(
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " "rcl",
"failed to fini string_map (%d) during error handling: %s\n", "failed to fini string_map (%d) during error handling: %s",
rcutils_ret, rcutils_ret,
rcutils_get_error_string_safe()); rcutils_get_error_string_safe())
} }
if (ret == RCL_RET_BAD_ALLOC) { if (ret == RCL_RET_BAD_ALLOC) {
return ret; return ret;

View file

@ -16,6 +16,8 @@
#include <string> #include <string>
#include <thread> #include <thread>
#include "rcutils/logging_macros.h"
#include "rcl/client.h" #include "rcl/client.h"
#include "rcl/rcl.h" #include "rcl/rcl.h"
@ -39,9 +41,9 @@ 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) {
fprintf( RCUTILS_LOG_ERROR(
stderr, "Error in rcl_service_server_is_available: %s\n", "Error in rcl_service_server_is_available: %s",
rcl_get_error_string_safe()); rcl_get_error_string_safe())
return false; return false;
} }
if (is_ready) { if (is_ready) {
@ -61,12 +63,12 @@ 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) {
fprintf(stderr, "Error in wait set init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in wait set init: %s", rcl_get_error_string_safe())
return false; return false;
} }
auto wait_set_exit = make_scope_exit([&wait_set]() { auto wait_set_exit = make_scope_exit([&wait_set]() {
if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
fprintf(stderr, "Error in wait set fini: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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");
} }
}); });
@ -74,11 +76,11 @@ 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) {
fprintf(stderr, "Error in wait_set_clear_clients: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
fprintf(stderr, "Error in wait_set_add_client: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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));
@ -86,7 +88,7 @@ wait_for_client_to_be_ready(
continue; continue;
} }
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
fprintf(stderr, "Error in wait: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
@ -103,19 +105,19 @@ 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) {
fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in node init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto node_exit = make_scope_exit([&main_ret, &node]() { auto node_exit = make_scope_exit([&main_ret, &node]() {
if (rcl_node_fini(&node) != RCL_RET_OK) { if (rcl_node_fini(&node) != RCL_RET_OK) {
fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in node fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
@ -128,20 +130,20 @@ 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) {
fprintf(stderr, "Error in client init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in client init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto client_exit = make_scope_exit([&client, &main_ret, &node]() { auto client_exit = make_scope_exit([&client, &main_ret, &node]() {
if (rcl_client_fini(&client, &node)) { if (rcl_client_fini(&client, &node)) {
fprintf(stderr, "Error in client fini: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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)) {
fprintf(stderr, "Server never became available\n"); RCUTILS_LOG_ERROR("Server never became available")
return -1; return -1;
} }
@ -153,12 +155,12 @@ 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)) {
fprintf(stderr, "Error in send request: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in send request: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
if (sequence_number != 1) { if (sequence_number != 1) {
fprintf(stderr, "Got invalid sequence number\n"); RCUTILS_LOG_ERROR("Got invalid sequence number")
return -1; return -1;
} }
@ -169,12 +171,12 @@ 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)) {
fprintf(stderr, "Client never became ready\n"); RCUTILS_LOG_ERROR("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) {
fprintf(stderr, "Error in send response: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in send response: %s", rcl_get_error_string_safe())
return -1; return -1;
} }

View file

@ -16,6 +16,8 @@
#include <string> #include <string>
#include <thread> #include <thread>
#include "rcutils/logging_macros.h"
#include "rcl/service.h" #include "rcl/service.h"
#include "rcl/rcl.h" #include "rcl/rcl.h"
@ -35,12 +37,12 @@ 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) {
fprintf(stderr, "Error in wait set init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in wait set init: %s", rcl_get_error_string_safe())
return false; return false;
} }
auto wait_set_exit = make_scope_exit([&wait_set]() { auto wait_set_exit = make_scope_exit([&wait_set]() {
if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
fprintf(stderr, "Error in wait set fini: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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");
} }
}); });
@ -48,11 +50,11 @@ 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) {
fprintf(stderr, "Error in wait_set_clear_services: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
fprintf(stderr, "Error in wait_set_add_service: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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));
@ -60,7 +62,7 @@ wait_for_service_to_be_ready(
continue; continue;
} }
if (ret != RCL_RET_OK) { if (ret != RCL_RET_OK) {
fprintf(stderr, "Error in wait: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
@ -77,19 +79,19 @@ 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) {
fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in node init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto node_exit = make_scope_exit([&main_ret, &node]() { auto node_exit = make_scope_exit([&main_ret, &node]() {
if (rcl_node_fini(&node) != RCL_RET_OK) { if (rcl_node_fini(&node) != RCL_RET_OK) {
fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in node fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
@ -102,13 +104,13 @@ 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) {
fprintf(stderr, "Error in service init: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in service init: %s", rcl_get_error_string_safe())
return -1; return -1;
} }
auto service_exit = make_scope_exit([&main_ret, &service, &node]() { auto service_exit = make_scope_exit([&main_ret, &service, &node]() {
if (rcl_service_fini(&service, &node)) { if (rcl_service_fini(&service, &node)) {
fprintf(stderr, "Error in service fini: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("Error in service fini: %s", rcl_get_error_string_safe())
main_ret = -1; main_ret = -1;
} }
}); });
@ -123,7 +125,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)) {
fprintf(stderr, "Service never became ready\n"); RCUTILS_LOG_ERROR("Service never became ready")
return -1; return -1;
} }
@ -136,14 +138,14 @@ 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) {
fprintf(stderr, "Error in take_request: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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) {
fprintf(stderr, "Error in send_response: %s\n", rcl_get_error_string_safe()); RCUTILS_LOG_ERROR("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

View file

@ -30,6 +30,8 @@
#include "rcl/rcl.h" #include "rcl/rcl.h"
#include "rcl/graph.h" #include "rcl/graph.h"
#include "rcutils/logging_macros.h"
#include "std_msgs/msg/string.h" #include "std_msgs/msg/string.h"
#include "example_interfaces/srv/add_two_ints.h" #include "example_interfaces/srv/add_two_ints.h"
@ -242,12 +244,12 @@ check_graph_state(
bool expected_in_tnat, bool expected_in_tnat,
size_t number_of_tries) size_t number_of_tries)
{ {
printf( RCUTILS_LOG_INFO(
"Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.\n", "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,
expected_in_tnat ? "" : " not" expected_in_tnat ? "" : " not"
); )
size_t publisher_count = 0; size_t publisher_count = 0;
size_t subscriber_count = 0; size_t subscriber_count = 0;
bool is_in_tnat = false; bool is_in_tnat = false;
@ -280,19 +282,19 @@ check_graph_state(
rcl_reset_error(); rcl_reset_error();
} }
printf( RCUTILS_LOG_INFO(
" Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.\n", " Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
i + 1, i + 1,
publisher_count, publisher_count,
subscriber_count, subscriber_count,
is_in_tnat ? "" : " not" is_in_tnat ? "" : " not"
); )
if ( if (
expected_publisher_count == publisher_count && expected_publisher_count == publisher_count &&
expected_subscriber_count == subscriber_count && expected_subscriber_count == subscriber_count &&
expected_in_tnat == is_in_tnat) expected_in_tnat == is_in_tnat)
{ {
printf(" state correct!\n"); RCUTILS_LOG_INFO(" state correct!")
break; break;
} }
// Wait for graph change before trying again. // Wait for graph change before trying again.
@ -305,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);
printf( RCUTILS_LOG_INFO(
" 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) {
printf("timeout\n"); RCUTILS_LOG_INFO("timeout")
continue; continue;
} }
printf("change occurred\n"); RCUTILS_LOG_INFO("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);
@ -332,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());
printf("Using topic name: %s\n", topic_name.c_str()); RCUTILS_LOG_INFO("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);
@ -465,9 +467,9 @@ 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);
printf( RCUTILS_LOG_INFO(
"waiting up to '%s' nanoseconds for graph changes\n", "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());
if (ret == RCL_RET_TIMEOUT) { if (ret == RCL_RET_TIMEOUT) {
continue; continue;
@ -524,9 +526,9 @@ 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();
printf( RCUTILS_LOG_INFO(
"waiting up to '%s' nanoseconds for graph changes\n", "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());
if (ret == RCL_RET_TIMEOUT) { if (ret == RCL_RET_TIMEOUT) {
if (!is_connext) { if (!is_connext) {

View file

@ -27,6 +27,8 @@
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#include "rcl/wait.h" #include "rcl/wait.h"
#include "rcutils/logging_macros.h"
#ifdef RMW_IMPLEMENTATION #ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
@ -258,7 +260,8 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade
} else { } else {
std::stringstream ss; std::stringstream ss;
ss << "[thread " << test_set.thread_id << "] Timeout (try #" << wake_try_count << ")"; ss << "[thread " << test_set.thread_id << "] Timeout (try #" << wake_try_count << ")";
printf("%s\n", ss.str().c_str()); // TODO(mikaelarguedas) replace this with stream logging once they exist
RCUTILS_LOG_INFO("%s", ss.str().c_str())
} }
} }
if (!change_detected) { if (!change_detected) {

View file

@ -150,7 +150,9 @@ rcl_lifecycle_is_valid_transition(
return &current_state->valid_transitions[i]; return &current_state->valid_transitions[i];
} }
} }
RCUTILS_LOG_WARN("No callback transition matching %d found for current state %s", RCUTILS_LOG_WARN_NAMED(
"rcl_lifecycle",
"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;
} }
@ -165,14 +167,17 @@ 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("No transition found for node %s with key %d", RCUTILS_LOG_ERROR_NAMED(
"rcl_lifecycle",
"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());
return RCL_RET_ERROR; return RCL_RET_ERROR;
} }
if (!transition->goal) { if (!transition->goal) {
RCUTILS_LOG_ERROR("No valid goal is set") RCUTILS_LOG_ERROR_NAMED(
"rcl_lifecycle", "No valid goal is set")
} }
state_machine->current_state = transition->goal; state_machine->current_state = transition->goal;
if (publish_notification) { if (publish_notification) {
@ -193,13 +198,17 @@ 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) {
printf("Primary State: %s(%u)\n", map->states[i].label, map->states[i].id); RCUTILS_LOG_INFO_NAMED(
printf("# of valid transitions: %u\n", map->states[i].valid_transition_size); "rcl_lifecycle",
"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) { for (size_t j = 0; j < map->states[i].valid_transition_size; ++j) {
printf("\tNode %s: Key %d: Transition: %s\n", RCUTILS_LOG_INFO("\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)
} }
} }
} }