From fdbb852900aa4fdf78c9878c11e604a213f4710c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 7 Jul 2023 14:29:17 +0200 Subject: [PATCH] Backport: Switch lifecycle to use the RCLCPP macros --- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- .../src/lifecycle_node_interface_impl.hpp | 38 +++++++++++++------ 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 06378b594b..333edf41b8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -107,7 +107,7 @@ LifecycleNode::LifecycleNode( )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), - impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_)) { impl_->init(enable_communication_interface); diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 60570d4059..13110d29c7 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -39,6 +39,7 @@ #include "rcl_lifecycle/transition_map.h" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rcutils/logging_macros.h" @@ -57,9 +58,11 @@ class LifecycleNode::LifecycleNodeInterfaceImpl public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface) + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface) : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) + node_services_interface_(node_services_interface), + node_logging_interface_(node_logging_interface) {} ~LifecycleNodeInterfaceImpl() @@ -71,8 +74,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); } if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", + RCLCPP_FATAL( + node_logging_interface_->get_logger(), "failed to destroy rcl_state_machine"); } } @@ -402,7 +405,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl { std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Unable to change state for state machine for %s: %s", node_base_interface_->get_name(), rcl_get_error_string().str); return RCL_RET_ERROR; @@ -414,7 +418,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rcl_lifecycle_trigger_transition_by_id( &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Unable to start transition %u from current state %s: %s", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); rcutils_reset_error(); @@ -443,7 +448,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rcl_lifecycle_trigger_transition_by_label( &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Failed to finish transition %u. Current state is now: %s (%s)", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); rcutils_reset_error(); @@ -455,7 +461,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); + RCLCPP_WARN( + node_logging_interface_->get_logger(), + "Error occurred while doing error handling."); auto error_cb_code = execute_callback(current_state_id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code); @@ -464,7 +472,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rcl_lifecycle_trigger_transition_by_label( &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Failed to call cleanup on error state: %s", rcl_get_error_string().str); rcutils_reset_error(); return RCL_RET_ERROR; } @@ -487,8 +497,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl try { cb_success = callback(State(previous_state)); } catch (const std::exception & e) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Caught exception in callback for transition %d", it->first); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Original error: %s", e.what()); cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } @@ -576,6 +590,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; + using NodeLoggingPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; using GetAvailableStatesSrvPtr = @@ -587,6 +602,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; + NodeLoggingPtr node_logging_interface_; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_;