Added tool to request transition to ErrorState

nav2_util/include/nav2_util/lifecycle_utils.hpp

@@ -77,6 +77,33 @@ void reset_lifecycle_nodes(
   reset_lifecycle_nodes(split(nodes, ':'), service_call_timeout, retries);
 }
 
+/// Transition the given lifecycle nodes to the ErrorProcessing state in order
+/** At this time, service calls frequently hang for unknown reasons. The only
+ *  way to combat that is to timeout the service call and retry it. To use this
+ *  function, estimate how long your nodes should take to at each transition and
+ *  set your timeout accordingly.
+ * \param[in] node_names A vector of the fully qualified node names to reset.
+ * \param[in] service_call_timeout The maximum amount of time to wait for a
+ *            service call.
+ * \param[in] retries The number of times to try a state transition service call
+ */
+void process_error_lifecycle_nodes(
+  const std::vector<std::string> & node_names,
+  const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(),
+  const int retries = 3);
+
+/// Transition the given lifecycle nodes to the ErrorProcessing state in order.
+/**
+ * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
+ */
+void process_error_lifecycle_nodes(
+  const std::string & nodes,
+  const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(),
+  const int retries = 3)
+{
+  process_error_lifecycle_nodes(split(nodes, ':'), service_call_timeout, retries);
+}
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__LIFECYCLE_UTILS_HPP_

nav2_util/src/lifecycle_utils.cpp

@@ -23,6 +23,7 @@
 
 using std::string;
 using lifecycle_msgs::msg::Transition;
+using lifecycle_msgs::msg::State;
 
 namespace nav2_util
 {
@@ -98,4 +99,65 @@ void reset_lifecycle_nodes(
   }
 }
 
+static void processErrorLifecycleNode(
+  const std::string & node_name,
+  const std::chrono::seconds service_call_timeout,
+  const int retries)
+{
+  LifecycleServiceClient sc(node_name);
+  uint8_t transition;
+  // Each state of the lifecycle node has a specific transition
+  // to the ErrorProcessing state. Some states do not support transitions
+  // to ErrorProcessing.
+  switch (sc.get_state(service_call_timeout)) {
+    case State::PRIMARY_STATE_UNKNOWN:
+      return;
+    case State::PRIMARY_STATE_UNCONFIGURED:
+      return;
+    case State::PRIMARY_STATE_INACTIVE:
+      return;
+    case State::PRIMARY_STATE_ACTIVE:
+      return;
+    case State::PRIMARY_STATE_FINALIZED:
+      return;
+    case State::TRANSITION_STATE_CONFIGURING:
+      transition = Transition::TRANSITION_ON_CONFIGURE_ERROR;
+      break;
+    case State::TRANSITION_STATE_CLEANINGUP:
+      transition = Transition::TRANSITION_ON_CLEANUP_ERROR;
+      break;
+    case State::TRANSITION_STATE_SHUTTINGDOWN:
+      transition = Transition::TRANSITION_ON_SHUTDOWN_ERROR;
+      break;
+    case State::TRANSITION_STATE_ACTIVATING:
+      transition = Transition::TRANSITION_ON_ACTIVATE_ERROR;
+      break;
+    case State::TRANSITION_STATE_DEACTIVATING:
+      transition = Transition::TRANSITION_ON_DEACTIVATE_ERROR;
+      break;
+    case State::TRANSITION_STATE_ERRORPROCESSING:
+      transition = Transition::TRANSITION_ON_ERROR_ERROR;
+      break;
+    default:
+      return;
+  }
+
+  // Despite waiting for the service to be available and using reliable transport
+  // service calls still frequently hang. To get reliable reset it's necessary
+  // to timeout the service call and retry it when that happens.
+  RETRY(
+    sc.change_state(transition, service_call_timeout),
+    retries);
+}
+
+void process_error_lifecycle_nodes(
+  const std::vector<std::string> & node_names,
+  const std::chrono::seconds service_call_timeout,
+  const int retries)
+{
+  for (const auto & node_name : node_names) {
+    processErrorLifecycleNode(node_name, service_call_timeout, retries);
+  }
+}
+
 }  // namespace nav2_util