From 9c49e996f0737f9b4b2c3abd0c177a5eb034aba7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 6 Jul 2023 13:23:02 +0000 Subject: [PATCH] Switch lifecycle to use the RCLCPP macros. This ensures that they'll go out to /rosout and the disk. Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- .../src/lifecycle_node_interface_impl.cpp | 36 +++++++++++++------ .../src/lifecycle_node_interface_impl.hpp | 6 +++- 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4c0b94cb42..531b582734 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -115,7 +115,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.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 5c5f7797e1..9074c9cc6e 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_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -50,9 +51,11 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNodeInterfaceImpl::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) { } @@ -65,8 +68,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"); } } @@ -398,7 +401,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; @@ -411,7 +415,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(); @@ -443,7 +448,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(); @@ -458,7 +464,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); @@ -467,7 +475,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; } @@ -495,8 +505,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 d09f44831c..5cf5bdaacf 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" @@ -52,7 +53,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final 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); ~LifecycleNodeInterfaceImpl(); @@ -152,6 +154,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 = @@ -163,6 +166,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_;