Allow stopping missions from idle state

equinor · Oct 24, 2024 · 89a1d09 · 89a1d09
1 parent 15afff2
commit 89a1d09
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 14 deletions.
diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py
@@ -21,13 +21,7 @@
 from isar.services.utilities.scheduling_utilities import SchedulingUtilities
 from isar.state_machine.states_enum import States
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.task import (
-    TASKS,
-    Localize,
-    MoveArm,
-    ReturnToHome,
-)
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Localize, MoveArm, ReturnToHome
 
 
 class SchedulingController:
@@ -192,7 +186,7 @@ def stop_mission(self) -> ControlMissionResponse:
 
         state: States = self.scheduling_utilities.get_state()
 
-        if state in [States.Off, States.Idle]:
+        if state in [States.Off]:
             error_message = (
                 f"Conflict - Stop command received in invalid state - State: {state}"
             )

diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py
@@ -32,11 +32,7 @@
 from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
 from robot_interface.models.initialize.initialize_params import InitializeParams
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import (
-    MissionStatus,
-    RobotStatus,
-    TaskStatus,
-)
+from robot_interface.models.mission.status import MissionStatus, RobotStatus, TaskStatus
 from robot_interface.models.mission.task import TASKS, Task
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
@@ -134,7 +130,11 @@ def __init__(
                 },
                 {
                     "trigger": "stop",
-                    "source": [self.initiate_state, self.monitor_state],
+                    "source": [
+                        self.initiate_state,
+                        self.monitor_state,
+                        self.idle_state,
+                    ],
                     "dest": self.stop_state,
                     "before": self._stop,
                 },
@@ -371,6 +371,11 @@ def _initiate_infeasible(self) -> None:
             self.iterate_current_task()
 
     def _mission_stopped(self) -> None:
+        if self.current_mission is None:
+            self._queue_empty_response()
+            self.reset_state_machine()
+            return
+
         self.current_mission.status = MissionStatus.Cancelled
 
         for task in self.current_mission.tasks:
@@ -588,6 +593,16 @@ def _make_control_mission_response(self) -> ControlMissionResponse:
             task_status=self.current_task.status,
         )
 
+    def _queue_empty_response(self):
+        self.queues.stop_mission.output.put(
+            ControlMissionResponse(
+                mission_id="None",
+                mission_status="None",
+                task_id="None",
+                task_status="None",
+            )
+        )
+
 
 def main(state_machine: StateMachine):
     """Starts a state machine instance."""

diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py
@@ -36,6 +36,9 @@ def stop(self) -> None:
 
     def _run(self) -> None:
         while True:
+            if self.state_machine.should_stop_mission():
+                transition = self.state_machine.stop  # type: ignore
+                break
             start_mission: Optional[StartMissionMessage] = (
                 self.state_machine.should_start_mission()
             )