[language] Interrupts and Monitors

src/pycram/enums.py

@@ -61,4 +61,6 @@ class State(Enum):
     """
     SUCCEEDED = 1
     FAILED = 0
+    RUNNING = 2
+    INTERRUPTED = 3
 

src/pycram/language.py

@@ -1,7 +1,7 @@
 # used for delayed evaluation of typing until python 3.11 becomes mainstream
 from __future__ import annotations
 
-from typing import Iterable, Optional, Callable, Dict, Any
+from typing import Iterable, Optional, Callable, Dict, Any, List
 from anytree import NodeMixin, Node, PreOrderIter
 
 from .enums import State
@@ -17,6 +17,7 @@ class Language(NodeMixin):
     """
     parallel_blocklist = ["PickUpAction", "PlaceAction", "OpenAction", "CloseAction", "TransportAction", "GraspingAction"]
     do_not_use_giskard = ["SetGripperAction", "MoveGripperMotion", "DetectAction", "DetectingMotion"]
+    block_list = []
 
     def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None):
         """
@@ -27,8 +28,11 @@ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = Non
         """
         self.parent = parent
         self.exceptions = {}
+        self.state = None
+        self.executing_thread = {}
+        self.interrupted = False
         if children:
-            self.children = children
+            self.children: Language = children
 
     def resolve(self) -> Language:
         """
@@ -100,6 +104,9 @@ def __xor__(self, other: Language) -> TryAll:
                 f"You can not execute the Designator {self if self.__class__.__name__ in self.parallel_blocklist else other} in a try all language expression.")
         return TryAll(parent=None, children=(self, other)).simplify()
 
+    def __rshift__(self, other):
+        return Monitor(parent=None, children=(self, other)).simplify()
+
     def simplify(self) -> Language:
         """
         Simplifies the language tree by merging which have a parent-child relation and are of the same type.
@@ -139,6 +146,22 @@ def merge_nodes(node1: Node, node2: Node) -> None:
         node2.parent = None
         node1.children = node2.children + node1.children
 
+    def interrupt(self):
+        raise NotImplementedError
+
+
+class Monitor(Language):
+    def __init__(self, parent=None, children=None, condition=None):
+        super().__init__(parent, children)
+        self.condition = condition
+
+    def perform(self):
+        ...
+
+    def interrupt(self):
+        for child in self.children:
+            child.interrupt()
+
 
 class Sequential(Language):
     """
@@ -158,12 +181,22 @@ def perform(self) -> State:
         """
         try:
             for child in self.children:
+                if self.interrupted:
+                    if threading.get_ident() in self.block_list:
+                        self.block_list.remove(threading.get_ident())
+                    return
+                self.root.executing_thread[child] = threading.get_ident()
                 child.resolve().perform()
         except PlanFailure as e:
             self.root.exceptions[self] = e
             return State.FAILED
         return State.SUCCEEDED
 
+    def interrupt(self):
+        self.interrupted = True
+        self.block_list.append(threading.get_ident())
+        giskard.giskard_wrapper.interrupt()
+
 
 class TryInOrder(Language):
     """
@@ -183,6 +216,10 @@ def perform(self) -> State:
         """
         failure_list = []
         for child in self.children:
+            if self.interrupted:
+                if threading.get_ident() in self.block_list:
+                    self.block_list.remove(threading.get_ident())
+                return
             try:
                 child.resolve().perform()
             except PlanFailure as e:
@@ -195,6 +232,11 @@ def perform(self) -> State:
         else:
             return State.SUCCEEDED
 
+    def interrupt(self):
+        self.interrupted = True
+        self.block_list.append(threading.get_ident())
+        giskard.giskard_wrapper.interrupt()
+
 
 class Parallel(Language):
     """
@@ -213,7 +255,7 @@ def perform(self) -> State:
 
         :return: The state according to the behaviour described in :func:`Parallel`
         """
-        threads = []
+        self.threads: List[threading.Thread] = []
 
         def lang_call(child_node):
             if ("DesignatorDescription" in [cls.__name__ for cls in child_node.__class__.__mro__]
@@ -223,6 +265,7 @@ def lang_call(child_node):
                 else:
                     giskard.par_threads[self].append(threading.get_ident())
             try:
+                self.root.executing_thread[child] = threading.get_ident()
                 child_node.resolve().perform()
             except PlanFailure as e:
                 if self in self.root.exceptions.keys():
@@ -231,15 +274,24 @@ def lang_call(child_node):
                     self.root.exceptions[self] = [e]
 
         for child in self.children:
+            if self.interrupted:
+                break
             t = threading.Thread(target=lambda: lang_call(child))
             t.start()
-            threads.append(t)
-        for thread in threads:
+            self.threads.append(t)
+        for thread in self.threads:
             thread.join()
+            if thread.ident in self.block_list:
+                self.block_list.remove(thread.ident)
         if self in self.root.exceptions.keys() and len(self.root.exceptions[self]) != 0:
             return State.FAILED
         return State.SUCCEEDED
 
+    def interrupt(self):
+        self.interrupted = True
+        self.block_list += [t.ident for t in self.threads]
+        giskard.giskard_wrapper.interrupt()
+
 
 class TryAll(Language):
     """
@@ -257,7 +309,7 @@ def perform(self) -> State:
 
         :return: The state according to the behaviour described in :func:`TryAll`
         """
-        threads = []
+        self.threads: List[threading.Thread] = []
         failure_list = []
 
         def lang_call(child_node):
@@ -279,15 +331,22 @@ def lang_call(child_node):
         for child in self.children:
             t = threading.Thread(target=lambda: lang_call(child))
             t.start()
-            threads.append(t)
-        for thread in threads:
+            self.threads.append(t)
+        for thread in self.threads:
             thread.join()
+            if thread.ident in self.block_list:
+                self.block_list.remove(thread.ident)
         if len(self.children) == len(failure_list):
             self.root.exceptions[self] = failure_list
             return State.FAILED
         else:
             return State.SUCCEEDED
 
+    def interrupt(self):
+        self.interrupted = True
+        self.block_list += [t.ident for t in self.threads]
+        giskard.giskard_wrapper.interrupt()
+
 
 class Code(Language):
     """

src/pycram/process_module.py

@@ -7,6 +7,7 @@
 from __future__ import annotations
 
 import inspect
+import threading
 import time
 from abc import ABC
 from threading import Lock
@@ -16,8 +17,9 @@
 from .designator import MotionDesignatorDescription
 from .fluent import Fluent
 from typing import Callable, List, Type, Any, Union
+from .language import Language
 
-from .robot_descriptions import  robot_description
+from .robot_descriptions import robot_description
 
 
 class ProcessModule:
@@ -28,6 +30,11 @@ class ProcessModule:
     """
     Adds a delay of 0.5 seconds after executing a process module, to make the execution in simulation more realistic
     """
+    block_list = []
+    """
+    List of thread ids for which no Process Modules should be executed. This is used as an interrupt mechanism for 
+    Designators
+    """
 
     def __init__(self, lock):
         """Create a new process module."""
@@ -49,6 +56,8 @@ def execute(self, designator: MotionDesignatorDescription.Motion) -> Any:
         :param designator: The designator to execute.
         :return: Return of the Process Module if there is any
         """
+        if threading.get_ident() in Language.block_list:
+            return None
         with self._lock:
             ret = self._execute(designator)
             if ProcessModule.execution_delay:

src/pycram/robot_descriptions/pr2_description.py

@@ -97,6 +97,9 @@ def __init__(self):
                                            "right": [0, 0, 1, 1],
                                            "top": [0, 1, 0, 1]})
 
+        self.gripper_action_topics = {"left": "l_gripper_controller/gripper_action",
+                                      "right": "r_gripper_controller/gripper_action"}
+
     def get_camera_frame(self, name="optical_frame"):
         # TODO: Hacky since only one optical camera frame from pr2 is used
         return super().get_camera_frame(name)