From c540c0d57eb02a9a6bf871b76e87db9192d49e77 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Chirico=20Indreb=C3=B8?= <ACIN@equinor.com>
Date: Wed, 6 Mar 2024 10:22:43 +0100
Subject: [PATCH] Implement pause and stop in robotinterface

---
 src/isar_exr/api/energy_robotics_api.py | 47 +++++++++++++++++++++++-
 src/isar_exr/robotinterface.py          | 49 +++++++++++++++++++++++++
 2 files changed, 95 insertions(+), 1 deletion(-)

diff --git a/src/isar_exr/api/energy_robotics_api.py b/src/isar_exr/api/energy_robotics_api.py
index 9cdd15c..c8d8332 100644
--- a/src/isar_exr/api/energy_robotics_api.py
+++ b/src/isar_exr/api/energy_robotics_api.py
@@ -13,6 +13,7 @@
     RobotInfeasibleMissionException,
     RobotMapException,
     RobotMissionStatusException,
+    RobotActionException,
 )
 from robot_interface.models.mission.status import MissionStatus
 
@@ -146,7 +147,6 @@ def pause_current_mission(self, exr_robot_id: str) -> None:
             ).select(
                 self.schema.MissionExecutionType.id,
                 self.schema.MissionExecutionType.status,
-                self.schema.MissionExecutionType.failures,
             )
         )
 
@@ -158,6 +158,10 @@ def pause_current_mission(self, exr_robot_id: str) -> None:
             result: Dict[str, Any] = self.client.query(
                 dsl_gql(pause_current_mission_mutation), params
             )
+        except TransportQueryError as e:
+            raise RobotActionException(
+                f"Could not pause the running mission since it is in a conflicting state: {e}"
+            )
         except Exception:
             raise RobotCommunicationException(
                 error_description="Could not pause the running mission",
@@ -173,6 +177,47 @@ def pause_current_mission(self, exr_robot_id: str) -> None:
                 error_description=f"Invalid status after pausing mission: '{status}'"
             )
 
+    def resume_current_mission(self, exr_robot_id: str) -> None:
+        params: dict = {"robotID": exr_robot_id}
+
+        variable_definitions_graphql: DSLVariableDefinitions = DSLVariableDefinitions()
+
+        resume_current_mission_mutation: DSLMutation = DSLMutation(
+            self.schema.Mutation.resumeMissionExecution.args(
+                robotID=variable_definitions_graphql.robotID
+            ).select(
+                self.schema.MissionExecutionType.id,
+                self.schema.MissionExecutionType.status,
+            )
+        )
+
+        resume_current_mission_mutation.variable_definitions = (
+            variable_definitions_graphql
+        )
+
+        try:
+            result: Dict[str, Any] = self.client.query(
+                dsl_gql(resume_current_mission_mutation), params
+            )
+        except TransportQueryError as e:
+            raise RobotActionException(
+                f"Could not resume the running mission since it is in a conflicting state: {e}"
+            )
+        except Exception:
+            raise RobotCommunicationException(
+                error_description="Could not resume the running mission",
+            )
+
+        status: ExrMissionStatus = ExrMissionStatus(result["status"])
+        success: bool = status in [
+            ExrMissionStatus.Paused,
+            ExrMissionStatus.PauseRequested,
+        ]
+        if not success:
+            raise RobotMissionStatusException(
+                error_description=f"Invalid status after resuming mission: '{status}'"
+            )
+
     def get_point_of_interest_by_customer_tag(
         self, customer_tag: str, site_id: str
     ) -> str:
diff --git a/src/isar_exr/robotinterface.py b/src/isar_exr/robotinterface.py
index c87a39c..707a1a2 100644
--- a/src/isar_exr/robotinterface.py
+++ b/src/isar_exr/robotinterface.py
@@ -27,6 +27,7 @@
     RobotMissionNotSupportedException,
     RobotMissionStatusException,
     RobotStepStatusException,
+    RobotActionException,
 )
 from robot_interface.models.initialize import InitializeParams
 from robot_interface.models.inspection.inspection import Inspection
@@ -291,6 +292,54 @@ def stop(self) -> None:
                 error_description=message,
             )
 
+    def pause(self) -> None:
+        max_request_attempts: int = 10
+        pause_mission_attempts: int = 0
+        while pause_mission_attempts < max_request_attempts:
+            try:
+                self.api.pause_current_mission(self.exr_robot_id)
+            except RobotActionException as e:
+                self.logger.warning(
+                    f"Failed to pause current mission: {e.error_reason}"
+                )
+                pause_mission_attempts += 1
+                time.sleep(1)
+                continue
+            except Exception as e:
+                message: str = "Could not pause the running mission\n"
+                self.logger.error(message)
+                raise RobotCommunicationException(
+                    error_description=message,
+                )
+            return
+        raise RobotActionException(
+            f"Failed to pause current mission after {pause_mission_attempts} failed attempts"
+        )
+
+    def resume(self) -> None:
+        max_request_attempts: int = 10
+        resume_mission_attempts: int = 0
+        while resume_mission_attempts < max_request_attempts:
+            try:
+                self.api.resume_current_mission(self.exr_robot_id)
+            except RobotActionException as e:
+                self.logger.warning(
+                    f"Failed to resume current mission: {e.error_reason}"
+                )
+                resume_mission_attempts += 1
+                time.sleep(1)
+                continue
+            except Exception as e:
+                message: str = "Could not resume the running mission\n"
+                self.logger.error(message)
+                raise RobotCommunicationException(
+                    error_description=message,
+                )
+            return
+        raise RobotActionException(
+            f"Failed to resume current mission after {resume_mission_attempts} failed attempts"
+        )
+
     def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
         raise NotImplementedError