From 9213373262e37a2c3b4d834f1fabcbe808b3177a Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 24 Jul 2023 11:54:32 -0400 Subject: [PATCH] Switch lifecycle to use the RCLCPP macros. (backport #2233) (#2244) * Switch lifecycle to use the RCLCPP macros. (#2233) This ensures that they'll go out to /rosout and the disk. Signed-off-by: Chris Lalancette (cherry picked from commit 945d254e321381ecb618c680015ba2b38ce898eb) Signed-off-by: Ignacio Vizzo --- .../src/lifecycle_node_interface_impl.cpp | 31 +++++++++++++------ .../src/lifecycle_node_interface_impl.hpp | 3 ++ 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 8d3855aacf..0e825dcfd5 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -29,6 +29,7 @@ #include "lifecycle_msgs/srv/get_available_transitions.hpp" #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 "rclcpp/node_interfaces/node_type_descriptions.hpp" @@ -56,6 +57,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( std::shared_ptr node_services_interface) : node_base_interface_(node_base_interface), node_services_interface_(node_services_interface), + node_logging_interface_(node_logging_interface), node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( node_base_interface, node_logging_interface, @@ -73,8 +75,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::~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"); } } @@ -406,7 +408,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( { 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; @@ -419,7 +422,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( 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(); @@ -451,7 +455,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( 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(); @@ -466,7 +471,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( // 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); @@ -475,7 +482,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( 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; } @@ -503,8 +512,12 @@ LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( 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; } } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 5db7c31d99..52d721374d 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -32,6 +32,7 @@ #include "rclcpp/macros.hpp" #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 "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -157,6 +158,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final 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 = @@ -168,6 +170,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final 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_;