diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f6eb9d2..445f1d2 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -36,6 +36,8 @@ #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcutils/logging_macros.h" + #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -199,7 +201,9 @@ public: int64_t sequence_number = request_header->sequence_number; // TODO(esteve) this should throw instead since it is not expected to happen in the first place if (this->pending_requests_.count(sequence_number) == 0) { - fprintf(stderr, "Received invalid sequence number. Ignoring...\n"); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); return; } auto tuple = this->pending_requests_[sequence_number]; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 56ac07a..26919c1 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -25,6 +25,8 @@ #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcutils/logging_macros.h" + #include "rmw/types.h" namespace rclcpp @@ -187,35 +189,45 @@ public: { for (auto subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add subscription to wait set: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto client : client_handles_) { if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add client to wait set: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add client to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto service : service_handles_) { if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add service to wait set: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add service to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto timer : timer_handles_) { if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add timer to wait set: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add timer to wait set: %s", rcl_get_error_string_safe()); return false; } } for (auto guard_condition : guard_conditions_) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add guard_condition to wait set: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add guard_condition to wait set: %s", rcl_get_error_string_safe()); return false; } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index eb31da6..54e37f7 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -28,6 +28,8 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rcutils/logging_macros.h" + using rclcpp::executor::AnyExecutable; using rclcpp::executor::Executor; using rclcpp::executor::ExecutorArgs; @@ -59,12 +61,14 @@ Executor::Executor(const ExecutorArgs & args) if (rcl_wait_set_init( &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to create wait set: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to create wait set: %s", rcl_get_error_string_safe()); rcl_reset_error(); if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string_safe()); rcl_reset_error(); } throw std::runtime_error("Failed to create wait set in Executor constructor"); @@ -85,14 +89,16 @@ Executor::~Executor() // Finalize the wait set. if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy wait set: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy wait set: %s", rcl_get_error_string_safe()); rcl_reset_error(); } // Finalize the interrupt guard condition. if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string_safe()); rcl_reset_error(); } // Remove and release the sigint guard condition @@ -280,8 +286,9 @@ Executor::execute_subscription( message_info.from_intra_process = false; subscription->handle_message(message, message_info); } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take failed for subscription on topic '%s': %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take failed for subscription on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } @@ -303,8 +310,9 @@ Executor::execute_intra_process_subscription( message_info.from_intra_process = true; subscription->handle_intra_process_message(ipm, message_info); } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take failed for intra process subscription on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string_safe()); rcl_reset_error(); } @@ -330,8 +338,9 @@ Executor::execute_service( if (status == RCL_RET_OK) { service->handle_request(request_header, request); } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take request failed for server of service '%s': %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take request failed for server of service '%s': %s", service->get_service_name().c_str(), rcl_get_error_string_safe()); rcl_reset_error(); } @@ -350,8 +359,9 @@ Executor::execute_client( if (status == RCL_RET_OK) { client->handle_response(request_header, response); } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take response failed for client of service '%s': %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take response failed for client of service '%s': %s", client->get_service_name().c_str(), rcl_get_error_string_safe()); rcl_reset_error(); } @@ -439,7 +449,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_ret_t status = rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); if (status == RCL_RET_WAIT_SET_EMPTY) { - fprintf(stderr, "Warning: empty wait set received in rcl_wait(). This should never happen.\n"); + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in rcl_wait(). This should never happen."); } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { using rclcpp::exceptions::throw_from_rcl_error; throw_from_rcl_error(status, "rcl_wait() failed"); diff --git a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp index ca2787c..99fd858 100644 --- a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp +++ b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp @@ -20,6 +20,7 @@ #include "rcl/validate_topic_name.h" #include "rclcpp/exceptions.hpp" #include "rclcpp/scope_exit.hpp" +#include "rcutils/logging_macros.h" #include "rcutils/types/string_map.h" #include "rmw/error_handling.h" #include "rmw/validate_namespace.h" @@ -61,9 +62,9 @@ rclcpp::expand_topic_or_service_name( // finalize the string map before throwing rcutils_ret = rcutils_string_map_fini(&substitutions_map); if (rcutils_ret != RCUTILS_RET_OK) { - fprintf(stderr, - "[rclcpp|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini string_map (%d) during error handling: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to fini string_map (%d) during error handling: %s", rcutils_ret, rcutils_get_error_string_safe()); rcutils_reset_error(); diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 26c9c58..3e08e58 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -26,6 +26,8 @@ #include "rclcpp/node.hpp" #include "rmw/impl/cpp/demangle.hpp" +#include "rcutils/logging_macros.h" + using rclcpp::exceptions::throw_from_rcl_error; namespace rclcpp @@ -94,13 +96,16 @@ GraphListener::run() try { run_loop(); } catch (const std::exception & exc) { - fprintf(stderr, - "[rclcpp] caught %s exception in GraphListener thread: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "caught %s exception in GraphListener thread: %s", rmw::impl::cpp::demangle(exc).c_str(), exc.what()); std::rethrow_exception(std::current_exception()); } catch (...) { - fprintf(stderr, "[rclcpp] unknown error in GraphListener thread\n"); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "unknown error in GraphListener thread"); std::rethrow_exception(std::current_exception()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index ff05e69..26418c1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -20,6 +20,7 @@ #include "rclcpp/node_interfaces/node_base.hpp" #include "rclcpp/exceptions.hpp" +#include "rcutils/logging_macros.h" #include "rmw/validate_node_name.h" #include "rmw/validate_namespace.h" @@ -48,8 +49,9 @@ NodeBase::NodeBase( auto finalize_notify_guard_condition = [this]() { // Finalize the interrupt guard condition. if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string_safe()); } }; @@ -146,8 +148,9 @@ NodeBase::NodeBase( rcl_node, [](rcl_node_t * node) -> void { if (rcl_node_fini(node) != RCL_RET_OK) { - fprintf( - stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl node handle: %s", rcl_get_error_string_safe()); } delete node; }); @@ -167,8 +170,9 @@ NodeBase::~NodeBase() std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); notify_guard_condition_is_valid_ = false; if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string_safe()); } } } diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 406f927..a3d32b1 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -24,6 +24,7 @@ #include #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rcutils/logging_macros.h" #include "rmw/impl/cpp/demangle.hpp" #include "rclcpp/allocator/allocator_common.hpp" @@ -79,17 +80,17 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of intra process rcl publisher handle: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of intra process rcl publisher handle: %s", rcl_get_error_string_safe()); rcl_reset_error(); } if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of rcl publisher handle: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl publisher handle: %s", rcl_get_error_string_safe()); rcl_reset_error(); } diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index e387f6f..ea701e7 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -30,6 +30,8 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rcutils/logging_macros.h" + // Determine if sigaction is available #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE #define HAS_SIGACTION @@ -114,8 +116,9 @@ trigger_interrupt_guard_condition(int signal_value) for (auto & kv : g_sigint_guard_cond_handles) { rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second)); if (status != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to trigger guard condition: %s", rcl_get_error_string_safe()); } } } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 3d1a980..0d7f5b7 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -68,7 +68,9 @@ public: auto ret = rcl_lifecycle_state_machine_fini( &state_machine_, node_handle, &node_options->allocator); if (ret != RCL_RET_OK) { - fprintf(stderr, "FATAL: failed to destroy rcl_state_machine\n"); + RCUTILS_LOG_FATAL_NAMED( + "rclcpp_lifecycle", + "failed to destroy rcl_state_machine"); } }