diff --git a/lifecycle/src/lifecycle_service_client.cpp b/lifecycle/src/lifecycle_service_client.cpp index f3e8acee7..d5e6add1a 100644 --- a/lifecycle/src/lifecycle_service_client.cpp +++ b/lifecycle/src/lifecycle_service_client.cpp @@ -256,6 +256,34 @@ callee_script(std::shared_ptr lc_client) } } + // reconfigure it because it encountered an error + { + time_between_state_changes.sleep(); + if (!rclcpp::ok()) { + return; + } + if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) { + return; + } + if (!lc_client->get_state()) { + return; + } + } + + // activate it yet again + { + time_between_state_changes.sleep(); + if (!rclcpp::ok()) { + return; + } + if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) { + return; + } + if (!lc_client->get_state()) { + return; + } + } + // and deactivate it again { time_between_state_changes.sleep(); diff --git a/lifecycle/src/lifecycle_talker.cpp b/lifecycle/src/lifecycle_talker.cpp index 7ba607f57..27bd8f7bb 100644 --- a/lifecycle/src/lifecycle_talker.cpp +++ b/lifecycle/src/lifecycle_talker.cpp @@ -69,7 +69,7 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode * Callback for walltimer. This function gets invoked by the timer * and executes the publishing. * For this demo, we ask the node for its current state. If the - * lifecycle publisher is not activate, we still invoke publish, but + * lifecycle publisher is not activated, we still invoke publish, but * the communication is blocked so that no messages is actually transferred. */ void @@ -88,6 +88,15 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg->data.c_str()); } + // Simulate the node having some error whilst active, + // necessitating reconfiguration. + // raise_error() can be called from an inactive or active state. + if(count == 32){ + RCLCPP_ERROR(get_logger(),"An error has arisen in the node and it needs reconfiguration."); + raise_error(); + return; + } + // We independently from the current state call publish on the lifecycle // publisher. // Only if the publisher is in an active state, the message transfer is @@ -262,6 +271,40 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } + /// Transition callback for state error processing + /** + * on_error callback is being called when the lifecycle node + * enters the "errorProcessing" state. + * Depending on the return value of this function, the state machine + * either invokes a transition to the "unconfigured" state or the + * finalized state. + * TRANSITION_CALLBACK_SUCCESS transitions to "unconfigured" + * TRANSITION_CALLBACK_FAILURE transitions to "finalized" + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "finalized" + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State & state) + { + // We explicitly deactivate the lifecycle publisher. + // Starting from this point, all messages are no longer + // sent into the network. + timer_.reset(); + pub_.reset(); + + RCUTILS_LOG_ERROR_NAMED( + get_name(), + "on error is called from state %s.", + state.label().c_str()); + + // We return a success and hence invoke the transition to the next + // step: "unconfigured". + // If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine + // would go the "finalized" state. + // In case of TRANSITION_CALLBACK_ERROR or any thrown exception within + // this callback, the state machine also transitions to state "finalized". + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + private: // We hold an instance of a lifecycle publisher. This lifecycle publisher // can be activated or deactivated regarding on which state the lifecycle node