From 8c40675179e20e48a4d7074e059a9e951e9f17d7 Mon Sep 17 00:00:00 2001
From: thebyohazard <>
Date: Mon, 13 Apr 2020 15:19:48 -0500
Subject: [PATCH] Add example of lifecycle node having an error and needing
 reconfiguration Signed-off-by: thebyohazard <patrick@jlpengineering.com>

---
 lifecycle/src/lifecycle_service_client.cpp | 28 ++++++++++++++
 lifecycle/src/lifecycle_talker.cpp         | 45 +++++++++++++++++++++-
 2 files changed, 72 insertions(+), 1 deletion(-)

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<LifecycleServiceClient> 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..136e58f63 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