diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index cdf2990dde..94988e896e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -397,6 +397,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( constexpr bool publish_update = true; State initial_state; unsigned int current_state_id; + const rcl_lifecycle_transition_t * original_transition{nullptr}; { std::lock_guard lock(state_machine_mutex_); @@ -411,6 +412,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( // keep the initial state to pass to a transition callback initial_state = State(state_machine_.current_state); + original_transition = + rcl_lifecycle_get_transition_by_id(state_machine_.current_state, transition_id); + + if ( rcl_lifecycle_trigger_transition_by_id( &state_machine_, transition_id, publish_update) != RCL_RET_OK) @@ -461,12 +466,15 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( // Update the internal current_state_ current_state_ = State(state_machine_.current_state); - // error handling ?! + // error handling // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCLCPP_WARN( - node_logging_interface_->get_logger(), - "Error occurred while doing error handling."); + if (original_transition) { + RCLCPP_WARN( + node_logging_interface_->get_logger(), + "Callback returned ERROR during the transition: %s", original_transition->label); + } + auto error_cb_code = execute_callback(current_state_id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code);