From 1c35c184f48f83aec067a0e1eaf8b661e438fbbe Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Chirico=20Indreb=C3=B8?= <ACIN@equinor.com>
Date: Fri, 1 Mar 2024 09:38:30 +0100
Subject: [PATCH] Implement stop button

---
 src/isar_exr/api/energy_robotics_api.py | 41 +++++++++++++++++++++++++
 src/isar_exr/models/step_status.py      |  1 +
 src/isar_exr/robotinterface.py          | 28 ++++++++++++-----
 3 files changed, 62 insertions(+), 8 deletions(-)

diff --git a/src/isar_exr/api/energy_robotics_api.py b/src/isar_exr/api/energy_robotics_api.py
index b49341c..6f5d7c0 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
 
@@ -129,6 +130,46 @@ def pause_current_mission(self, exr_robot_id: str) -> None:
             raise RobotMissionStatusException(
                 error_description=f"Invalid status after pausing mission: '{status}'"
             )
+        
+    def stop_current_mission(self, exr_robot_id: str) -> None:
+        params: dict = {"robotID": exr_robot_id}
+
+        variable_definitions_graphql: DSLVariableDefinitions = DSLVariableDefinitions()
+
+        stop_current_mission_mutation: DSLMutation = DSLMutation(
+            self.schema.Mutation.resetMissionExecution.args(
+                robotID=variable_definitions_graphql.robotID
+            ).select(
+                self.schema.MissionExecutionType.id,
+                self.schema.MissionExecutionType.status,
+                self.schema.MissionExecutionType.failures,
+            )
+        )
+
+        stop_current_mission_mutation.variable_definitions = (
+            variable_definitions_graphql
+        )
+
+        try:
+            result: Dict[str, Any] = self.client.query(
+                dsl_gql(stop_current_mission_mutation), params
+            )
+        except TransportQueryError as e:
+            raise RobotActionException(f"Could not stop the running mission since it is in a conflicting state: {e}")
+        except Exception as e:
+            raise RobotCommunicationException(
+                error_description=f"Could not stop the running mission: {e}",
+            )
+
+        status: ExrMissionStatus = ExrMissionStatus(result["resetMissionExecution"]["status"])
+        success: bool = status in [
+            ExrMissionStatus.ResetRequested,
+            ExrMissionStatus.Completed,
+        ]
+        if not success:
+            raise RobotMissionStatusException(
+                error_description=f"Invalid status after stopping mission: '{status}'"
+            )
 
     def get_point_of_interest_by_customer_tag(
         self, customer_tag: str, site_id: str
diff --git a/src/isar_exr/models/step_status.py b/src/isar_exr/models/step_status.py
index 3163b0c..ad71bcf 100644
--- a/src/isar_exr/models/step_status.py
+++ b/src/isar_exr/models/step_status.py
@@ -7,6 +7,7 @@ class ExrMissionStatus(str, Enum):
     StartRequested: str = "START_REQUESTED"
     PauseRequested: str = "PAUSE_REQUESTED"
     ResumeRequested: str = "RESUME_REQUESTED"
+    ResetRequested: str = "RESET_REQUESTED"
     Rejected: str = "REJECTED"
     WakingUp: str = "WAKING_UP"
     Starting: str = "STARTING"
diff --git a/src/isar_exr/robotinterface.py b/src/isar_exr/robotinterface.py
index b857d27..b1eb8bf 100644
--- a/src/isar_exr/robotinterface.py
+++ b/src/isar_exr/robotinterface.py
@@ -25,6 +25,7 @@
     RobotInitializeException,
     RobotMissionNotSupportedException,
     RobotMissionStatusException,
+    RobotActionException,
 )
 from robot_interface.models.initialize import InitializeParams
 from robot_interface.models.inspection.inspection import Inspection
@@ -236,14 +237,25 @@ def step_status(self) -> StepStatus:
         raise NotImplementedError
 
     def stop(self) -> None:
-        try:
-            self.api.pause_current_mission(self.exr_robot_id)
-        except Exception:
-            message: str = "Could not stop the running mission\n"
-            self.logger.error(message)
-            raise RobotCommunicationException(
-                error_description=message,
-            )
+        max_request_attempts: int = 10
+        stop_mission_attempts: int = 0
+        while stop_mission_attempts < max_request_attempts:
+            try:
+                self.api.stop_current_mission(self.exr_robot_id)
+            except RobotActionException as e:
+                self.logger.warning(f"Failed to stop current mission: {e.error_reason}")
+                stop_mission_attempts += 1
+                time.sleep(1)
+                continue
+            except Exception as e:
+                message: str = "Could not stop the running mission\n"
+                self.logger.error(message)
+                raise RobotCommunicationException(
+                    error_description=message,
+                )
+            return
+        raise RobotActionException(f"Failed to stop current mission after {stop_mission_attempts} failed attempts")
+        
 
     def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
         raise NotImplementedError