use macros
This commit is contained in:
parent
3cff9020d1
commit
835a90761c
13 changed files with 133 additions and 94 deletions
|
@ -64,7 +64,7 @@ extern "C"
|
|||
* if (ret != RCL_RET_OK) {
|
||||
* // ... error handling
|
||||
* } 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:
|
||||
* allocator.deallocate(expanded_topic_name, allocator.state);
|
||||
* }
|
||||
|
|
|
@ -23,6 +23,7 @@ extern "C"
|
|||
#include <string.h>
|
||||
|
||||
#include "rcl/expand_topic_name.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rmw/error_handling.h"
|
||||
#include "rmw/rmw.h"
|
||||
#include "rmw/validate_full_topic_name.h"
|
||||
|
@ -91,11 +92,11 @@ rcl_client_init(
|
|||
if (ret != RCL_RET_OK) {
|
||||
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
|
||||
if (rcutils_ret != RCUTILS_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"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;
|
||||
|
|
|
@ -27,6 +27,7 @@ extern "C"
|
|||
#include "rcl/rcl.h"
|
||||
#include "rcutils/filesystem.h"
|
||||
#include "rcutils/get_env.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rcutils/macros.h"
|
||||
#include "rmw/error_handling.h"
|
||||
#include "rmw/node_security_options.h"
|
||||
|
@ -295,17 +296,19 @@ fail:
|
|||
if (node->impl->rmw_node_handle) {
|
||||
ret = rmw_destroy_node(node->impl->rmw_node_handle);
|
||||
if (ret != RMW_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"failed to fini rmw node in error recovery: %s\n", rmw_get_error_string_safe()
|
||||
);
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"failed to fini rmw node in error recovery: %s", rmw_get_error_string_safe()
|
||||
)
|
||||
}
|
||||
}
|
||||
if (node->impl->graph_guard_condition) {
|
||||
ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"failed to fini guard condition in error recovery: %s\n", rcl_get_error_string_safe()
|
||||
);
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"failed to fini guard condition in error recovery: %s", rcl_get_error_string_safe()
|
||||
)
|
||||
}
|
||||
allocator->deallocate(node->impl->graph_guard_condition, allocator->state);
|
||||
}
|
||||
|
|
|
@ -24,6 +24,7 @@ extern "C"
|
|||
|
||||
#include "./common.h"
|
||||
#include "rcl/expand_topic_name.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rmw/error_handling.h"
|
||||
#include "rmw/rmw.h"
|
||||
#include "rmw/validate_full_topic_name.h"
|
||||
|
@ -89,11 +90,11 @@ rcl_publisher_init(
|
|||
if (ret != RCL_RET_OK) {
|
||||
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
|
||||
if (rcutils_ret != RCUTILS_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
|
||||
"failed to fini string_map (%d) during error handling: %s\n",
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"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;
|
||||
|
|
|
@ -21,6 +21,7 @@ extern "C"
|
|||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <rcutils/logging_macros.h>
|
||||
#include <rmw/rmw.h>
|
||||
|
||||
#include "rcl/types.h"
|
||||
|
@ -54,20 +55,22 @@ INITIALIZER(initialize) {
|
|||
const char * expected = NULL;
|
||||
rcl_ret_t ret = rcl_impl_getenv("RCL_ASSERT_RMW_ID_MATCHES", &expected);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s\n",
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"Error getting environement variable 'RCL_ASSERT_RMW_ID_MATCHES': %s",
|
||||
rcl_get_error_string_safe()
|
||||
);
|
||||
)
|
||||
exit(ret);
|
||||
}
|
||||
// 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) {
|
||||
fprintf(stderr,
|
||||
"Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.\n",
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.",
|
||||
expected,
|
||||
rmw_get_implementation_identifier(),
|
||||
RCL_RET_MISMATCHED_RMW_ID
|
||||
);
|
||||
)
|
||||
exit(RCL_RET_MISMATCHED_RMW_ID);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -24,6 +24,7 @@ extern "C"
|
|||
|
||||
#include "./common.h"
|
||||
#include "rcl/expand_topic_name.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rmw/error_handling.h"
|
||||
#include "rmw/rmw.h"
|
||||
#include "rmw/validate_full_topic_name.h"
|
||||
|
@ -88,11 +89,11 @@ rcl_service_init(
|
|||
if (ret != RCL_RET_OK) {
|
||||
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
|
||||
if (rcutils_ret != RCUTILS_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
|
||||
"failed to fini string_map (%d) during error handling: %s\n",
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"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;
|
||||
|
@ -140,8 +141,10 @@ rcl_service_init(
|
|||
service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
|
||||
|
||||
if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
|
||||
fprintf(stderr, "Warning: Setting QoS durability to 'transient local' for service servers "
|
||||
"can cause them to receive requests from clients that have since terminated.\n");
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"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.
|
||||
// rmw handle (create rmw service)
|
||||
|
|
|
@ -69,6 +69,8 @@
|
|||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <rcutils/logging_macros.h>
|
||||
|
||||
// In MSVC, correct alignment of each type is already ensured.
|
||||
#define _Atomic(T) struct { T __val; }
|
||||
|
||||
|
@ -206,7 +208,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedCompareExchange8((char *) object, desired, *expected); \
|
||||
break; \
|
||||
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); \
|
||||
break; \
|
||||
} \
|
||||
|
@ -234,7 +237,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedExchange8((char *) object, desired); \
|
||||
break; \
|
||||
default: \
|
||||
fprintf(stderr, "Unsupported integer type in atomic_exchange_strong"); \
|
||||
RCUTILS_LOG_ERROR_NAMED( \
|
||||
"rcl", "Unsupported integer type in atomic_exchange_strong") \
|
||||
exit(-1); \
|
||||
break; \
|
||||
} \
|
||||
|
@ -259,7 +263,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedExchangeAdd8((char *) object, operand); \
|
||||
break; \
|
||||
default: \
|
||||
fprintf(stderr, "Unsupported integer type in atomic_fetch_add"); \
|
||||
RCUTILS_LOG_ERROR_NAMED( \
|
||||
"rcl", "Unsupported integer type in atomic_fetch_add") \
|
||||
exit(-1); \
|
||||
break; \
|
||||
} \
|
||||
|
@ -284,7 +289,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedAnd8((char *) object, operand); \
|
||||
break; \
|
||||
default: \
|
||||
fprintf(stderr, "Unsupported integer type in atomic_fetch_and"); \
|
||||
RCUTILS_LOG_ERROR_NAMED( \
|
||||
"rcl", "Unsupported integer type in atomic_fetch_and") \
|
||||
exit(-1); \
|
||||
break; \
|
||||
} \
|
||||
|
@ -309,7 +315,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedOr8((char *) object, operand); \
|
||||
break; \
|
||||
default: \
|
||||
fprintf(stderr, "Unsupported integer type in atomic_fetch_or"); \
|
||||
RCUTILS_LOG_ERROR_NAMED( \
|
||||
"rcl", "Unsupported integer type in atomic_fetch_or") \
|
||||
exit(-1); \
|
||||
break; \
|
||||
} \
|
||||
|
@ -337,7 +344,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedXor8((char *) object, operand); \
|
||||
break; \
|
||||
default: \
|
||||
fprintf(stderr, "Unsupported integer type in atomic_fetch_xor"); \
|
||||
RCUTILS_LOG_ERROR_NAMED( \
|
||||
"rcl", "Unsupported integer type in atomic_fetch_xor") \
|
||||
exit(-1); \
|
||||
break; \
|
||||
} \
|
||||
|
@ -362,7 +370,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t;
|
|||
out = _InterlockedExchangeAdd8((char *) object, 0); \
|
||||
break; \
|
||||
default: \
|
||||
fprintf(stderr, "Unsupported integer type in atomic_load"); \
|
||||
RCUTILS_LOG_ERROR_NAMED( \
|
||||
"rcl", "Unsupported integer type in atomic_load") \
|
||||
exit(-1); \
|
||||
break; \
|
||||
} \
|
||||
|
|
|
@ -23,6 +23,7 @@ extern "C"
|
|||
|
||||
#include "./common.h"
|
||||
#include "rcl/expand_topic_name.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rmw/error_handling.h"
|
||||
#include "rmw/rmw.h"
|
||||
#include "rmw/validate_full_topic_name.h"
|
||||
|
@ -83,11 +84,11 @@ rcl_subscription_init(
|
|||
if (ret != RCL_RET_OK) {
|
||||
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
|
||||
if (rcutils_ret != RCUTILS_RET_OK) {
|
||||
fprintf(stderr,
|
||||
"[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
|
||||
"failed to fini string_map (%d) during error handling: %s\n",
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rcl",
|
||||
"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;
|
||||
|
|
|
@ -16,6 +16,8 @@
|
|||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rcl/client.h"
|
||||
#include "rcl/rcl.h"
|
||||
|
||||
|
@ -39,9 +41,9 @@ 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) {
|
||||
fprintf(
|
||||
stderr, "Error in rcl_service_server_is_available: %s\n",
|
||||
rcl_get_error_string_safe());
|
||||
RCUTILS_LOG_ERROR(
|
||||
"Error in rcl_service_server_is_available: %s",
|
||||
rcl_get_error_string_safe())
|
||||
return false;
|
||||
}
|
||||
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_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator());
|
||||
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;
|
||||
}
|
||||
auto wait_set_exit = make_scope_exit([&wait_set]() {
|
||||
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");
|
||||
}
|
||||
});
|
||||
|
@ -74,11 +76,11 @@ wait_for_client_to_be_ready(
|
|||
do {
|
||||
++iteration;
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
|
||||
|
@ -86,7 +88,7 @@ wait_for_client_to_be_ready(
|
|||
continue;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
{
|
||||
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;
|
||||
}
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
auto node_exit = make_scope_exit([&main_ret, &node]() {
|
||||
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;
|
||||
}
|
||||
});
|
||||
|
@ -128,20 +130,20 @@ 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) {
|
||||
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;
|
||||
}
|
||||
|
||||
auto client_exit = make_scope_exit([&client, &main_ret, &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;
|
||||
}
|
||||
});
|
||||
|
||||
// Wait until server is available
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -153,12 +155,12 @@ int main(int argc, char ** argv)
|
|||
int64_t 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;
|
||||
}
|
||||
|
||||
if (sequence_number != 1) {
|
||||
fprintf(stderr, "Got invalid sequence number\n");
|
||||
RCUTILS_LOG_ERROR("Got invalid sequence number")
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
@ -169,12 +171,12 @@ int main(int argc, char ** argv)
|
|||
example_interfaces__srv__AddTwoInts_Response__init(&client_response);
|
||||
|
||||
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;
|
||||
}
|
||||
rmw_request_id_t header;
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
@ -16,6 +16,8 @@
|
|||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rcl/service.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_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator());
|
||||
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;
|
||||
}
|
||||
auto wait_set_exit = make_scope_exit([&wait_set]() {
|
||||
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");
|
||||
}
|
||||
});
|
||||
|
@ -48,11 +50,11 @@ wait_for_service_to_be_ready(
|
|||
do {
|
||||
++iteration;
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
|
||||
|
@ -60,7 +62,7 @@ wait_for_service_to_be_ready(
|
|||
continue;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
{
|
||||
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;
|
||||
}
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
auto node_exit = make_scope_exit([&main_ret, &node]() {
|
||||
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;
|
||||
}
|
||||
});
|
||||
|
@ -102,13 +104,13 @@ 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) {
|
||||
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;
|
||||
}
|
||||
|
||||
auto service_exit = make_scope_exit([&main_ret, &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;
|
||||
}
|
||||
});
|
||||
|
@ -123,7 +125,7 @@ int main(int argc, char ** argv)
|
|||
// Block until a client request comes in.
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -136,14 +138,14 @@ 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) {
|
||||
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;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
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;
|
||||
}
|
||||
// Our scope exits should take care of fini for everything
|
||||
|
|
|
@ -30,6 +30,8 @@
|
|||
#include "rcl/rcl.h"
|
||||
#include "rcl/graph.h"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "std_msgs/msg/string.h"
|
||||
#include "example_interfaces/srv/add_two_ints.h"
|
||||
|
||||
|
@ -242,12 +244,12 @@ check_graph_state(
|
|||
bool expected_in_tnat,
|
||||
size_t number_of_tries)
|
||||
{
|
||||
printf(
|
||||
"Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.\n",
|
||||
RCUTILS_LOG_INFO(
|
||||
"Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
|
||||
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;
|
||||
|
@ -280,19 +282,19 @@ check_graph_state(
|
|||
rcl_reset_error();
|
||||
}
|
||||
|
||||
printf(
|
||||
" Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.\n",
|
||||
RCUTILS_LOG_INFO(
|
||||
" Try %zu: %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
|
||||
i + 1,
|
||||
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)
|
||||
{
|
||||
printf(" state correct!\n");
|
||||
RCUTILS_LOG_INFO(" state correct!")
|
||||
break;
|
||||
}
|
||||
// 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);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
|
||||
printf(
|
||||
RCUTILS_LOG_INFO(
|
||||
" 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) {
|
||||
printf("timeout\n");
|
||||
RCUTILS_LOG_INFO("timeout")
|
||||
continue;
|
||||
}
|
||||
printf("change occurred\n");
|
||||
RCUTILS_LOG_INFO("change occurred")
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
}
|
||||
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::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch();
|
||||
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;
|
||||
const rcl_guard_condition_t * graph_guard_condition =
|
||||
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);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
|
||||
printf(
|
||||
"waiting up to '%s' nanoseconds for graph changes\n",
|
||||
std::to_string(time_to_sleep.count()).c_str());
|
||||
RCUTILS_LOG_INFO(
|
||||
"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());
|
||||
if (ret == RCL_RET_TIMEOUT) {
|
||||
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();
|
||||
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();
|
||||
printf(
|
||||
"waiting up to '%s' nanoseconds for graph changes\n",
|
||||
std::to_string(time_to_sleep.count()).c_str());
|
||||
RCUTILS_LOG_INFO(
|
||||
"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());
|
||||
if (ret == RCL_RET_TIMEOUT) {
|
||||
if (!is_connext) {
|
||||
|
|
|
@ -27,6 +27,8 @@
|
|||
#include "rcl/error_handling.h"
|
||||
#include "rcl/wait.h"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#ifdef RMW_IMPLEMENTATION
|
||||
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
|
||||
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
|
||||
|
@ -258,7 +260,8 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade
|
|||
} else {
|
||||
std::stringstream ss;
|
||||
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) {
|
||||
|
|
|
@ -150,7 +150,9 @@ rcl_lifecycle_is_valid_transition(
|
|||
return ¤t_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)
|
||||
return NULL;
|
||||
}
|
||||
|
@ -165,14 +167,17 @@ rcl_lifecycle_trigger_transition(
|
|||
|
||||
// If we have a faulty transition pointer
|
||||
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)
|
||||
RCL_SET_ERROR_MSG("Transition is not registered.", rcl_get_default_allocator());
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
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;
|
||||
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;
|
||||
for (size_t i = 0; i < map->states_size; ++i) {
|
||||
printf("Primary State: %s(%u)\n", map->states[i].label, map->states[i].id);
|
||||
printf("# of valid transitions: %u\n", map->states[i].valid_transition_size);
|
||||
RCUTILS_LOG_INFO_NAMED(
|
||||
"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) {
|
||||
printf("\tNode %s: Key %d: Transition: %s\n",
|
||||
RCUTILS_LOG_INFO("\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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue