From 4279ceffc927a30eacf79678ffa0bf8ce103b61c Mon Sep 17 00:00:00 2001
From: Simon Kristoffersson Lind <simon.kristoffersson_lind@cs.lth.se>
Date: Mon, 17 Jul 2023 11:21:58 +0200
Subject: [PATCH] Add RetryOnFail skill processor

This processor works like sequential, but if a child node fails, the
entire processor restarts from scratch.
An example use case might be a driving/navigation skill.
I.e. "if driving failed, try again X times".
---
 .../src/skiros2_skill/core/processors.py      | 36 +++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/skiros2_skill/src/skiros2_skill/core/processors.py b/skiros2_skill/src/skiros2_skill/core/processors.py
index 6f9ad58..1195c05 100644
--- a/skiros2_skill/src/skiros2_skill/core/processors.py
+++ b/skiros2_skill/src/skiros2_skill/core/processors.py
@@ -237,3 +237,39 @@ def processChildren(self, children, visitor):
             return state
         else:
             return State.Success
+
+class RetryOnFail():
+    """
+    @brief Like Sequential, but retries up to max_retries if a child node fails.
+           Restarts all child nodes on failure.
+    """
+
+    def __init__(self, max_retries=-1):
+        self.max_retries = max_retries
+        self.i = -1
+        self.index = 0
+
+    def printType(self):
+        return 'RetryOnFail({})'.format(self.max_retries)
+
+    def reset(self):
+        self.i = -1
+        self.index = 0
+
+    def processChildren(self, children, visitor):
+        
+        for i in range(self.index, len(children)):
+            c = children[i]
+            state = c.visit(visitor)
+            if state == State.Failure:
+                self.index = 0
+                self.i += 1
+                if self.i == self.max_retries:
+                    return State.Failure
+                else:
+                    return State.Running
+            elif state != State.Success:
+                return state
+            self.index += 1
+
+        return State.Success