isar 1.25.9__py3-none-any.whl → 1.26.0__py3-none-any.whl

isar/state_machine/state_machine.py

@@ -5,7 +5,6 @@ from collections import deque
 from datetime import datetime, timezone
 from typing import Deque, List, Optional
 
-from alitra import Pose
 from injector import inject
 from transitions import Machine
 from transitions.core import State
@@ -18,25 +17,38 @@ from isar.mission_planner.task_selector_interface import (
 )
 from isar.models.communication.message import StartMissionMessage
 from isar.models.communication.queues.queues import Queues
+from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
 from isar.state_machine.states.idle import Idle
-from isar.state_machine.states.initialize import Initialize
-from isar.state_machine.states.initiate import Initiate
 from isar.state_machine.states.monitor import Monitor
 from isar.state_machine.states.off import Off
 from isar.state_machine.states.offline import Offline
-from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
 from isar.state_machine.states.paused import Paused
 from isar.state_machine.states.stop import Stop
 from isar.state_machine.states_enum import States
+from isar.state_machine.transitions.fail_mission import (
+    report_failed_mission_and_finalize,
+)
+from isar.state_machine.transitions.finish_mission import finish_mission
+from isar.state_machine.transitions.pause import pause_mission
+from isar.state_machine.transitions.resume import resume_mission
+from isar.state_machine.transitions.start_mission import (
+    initialize_robot,
+    initiate_mission,
+    put_start_mission_on_queue,
+    set_mission_to_in_progress,
+    trigger_start_mission_or_task_event,
+)
+from isar.state_machine.transitions.stop import stop_mission
+from isar.state_machine.transitions.utils import def_transition
 from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
 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 RobotStatus, TaskStatus
 from robot_interface.models.mission.task import TASKS
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.telemetry.payloads import (
-    RobotStatusPayload,
     MissionPayload,
+    RobotStatusPayload,
     TaskPayload,
 )
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
@@ -85,9 +97,7 @@ class StateMachine(object):
         self.stop_state: State = Stop(self)
         self.paused_state: State = Paused(self)
         self.idle_state: State = Idle(self)
-        self.initialize_state: State = Initialize(self)
         self.monitor_state: State = Monitor(self)
-        self.initiate_state: State = Initiate(self)
         self.off_state: State = Off(self)
         self.offline_state: State = Offline(self)
         self.blocked_protective_stop: State = BlockedProtectiveStop(self)
@@ -95,8 +105,6 @@ class StateMachine(object):
         self.states: List[State] = [
             self.off_state,
             self.idle_state,
-            self.initialize_state,
-            self.initiate_state,
             self.monitor_state,
             self.stop_state,
             self.paused_state,
@@ -112,73 +120,67 @@ class StateMachine(object):
                     "source": self.off_state,
                     "dest": self.idle_state,
                 },
-                {
-                    "trigger": "initiated",
-                    "source": self.initiate_state,
-                    "dest": self.monitor_state,
-                    "before": self._initiated,
-                },
                 {
                     "trigger": "pause",
                     "source": self.monitor_state,
                     "dest": self.paused_state,
-                    "before": self._mission_paused,
+                    "before": def_transition(self, pause_mission),
                 },
                 {
                     "trigger": "stop",
                     "source": [
                         self.idle_state,
-                        self.initiate_state,
                         self.monitor_state,
                         self.paused_state,
                     ],
                     "dest": self.stop_state,
                 },
                 {
-                    "trigger": "mission_finished",
-                    "source": self.monitor_state,
-                    "dest": self.idle_state,
-                    "before": self._mission_finished,
-                },
-                {
-                    "trigger": "mission_started",
+                    "trigger": "request_mission_start",
                     "source": self.idle_state,
-                    "dest": self.initialize_state,
-                    "before": self._mission_started,
+                    "dest": self.monitor_state,
+                    "prepare": def_transition(self, put_start_mission_on_queue),
+                    "conditions": [
+                        def_transition(self, initiate_mission),
+                        def_transition(self, initialize_robot),
+                    ],
+                    "before": [
+                        def_transition(self, set_mission_to_in_progress),
+                        def_transition(self, trigger_start_mission_or_task_event),
+                    ],
                 },
                 {
-                    "trigger": "initialization_successful",
-                    "source": self.initialize_state,
-                    "dest": self.initiate_state,
-                    "before": self._initialization_successful,
+                    "trigger": "request_mission_start",
+                    "source": self.idle_state,
+                    "dest": self.idle_state,
                 },
                 {
-                    "trigger": "initialization_failed",
-                    "source": self.initialize_state,
+                    "trigger": "mission_failed_to_start",
+                    "source": self.monitor_state,
                     "dest": self.idle_state,
-                    "before": self._initialization_failed,
+                    "before": def_transition(self, report_failed_mission_and_finalize),
                 },
                 {
                     "trigger": "resume",
                     "source": self.paused_state,
                     "dest": self.monitor_state,
-                    "before": self._resume,
+                    "before": def_transition(self, resume_mission),
                 },
                 {
-                    "trigger": "initiate_failed",
-                    "source": self.initiate_state,
+                    "trigger": "mission_finished",
+                    "source": self.monitor_state,
                     "dest": self.idle_state,
-                    "before": self._initiate_failed,
+                    "before": def_transition(self, finish_mission),
                 },
                 {
                     "trigger": "mission_stopped",
                     "source": self.stop_state,
                     "dest": self.idle_state,
-                    "before": self._mission_stopped,
+                    "before": def_transition(self, stop_mission),
                 },
                 {
                     "trigger": "robot_turned_offline",
-                    "source": self.idle_state,
+                    "source": [self.idle_state, self.blocked_protective_stop],
                     "dest": self.offline_state,
                 },
                 {
@@ -188,7 +190,7 @@ class StateMachine(object):
                 },
                 {
                     "trigger": "robot_protective_stop_engaged",
-                    "source": self.idle_state,
+                    "source": [self.idle_state, self.offline_state],
                     "dest": self.blocked_protective_stop,
                 },
                 {
@@ -204,137 +206,14 @@ class StateMachine(object):
 
         self.current_mission: Optional[Mission] = None
         self.current_task: Optional[TASKS] = None
-        self.initial_pose: Optional[Pose] = None
+
+        self.mission_ongoing: bool = False
 
         self.current_state: State = States(self.state)  # type: ignore
 
         self.transitions_log_length: int = transitions_log_length
         self.transitions_list: Deque[States] = deque([], self.transitions_log_length)
 
-    #################################################################################
-    # Transition Callbacks
-    def _initialization_successful(self) -> None:
-        return
-
-    def _initialization_failed(self) -> None:
-        self.queues.start_mission.output.put(False)
-        self._finalize()
-
-    def _initiated(self) -> None:
-        self.current_mission.status = MissionStatus.InProgress
-        self.publish_task_status(task=self.current_task)
-        self.logger.info(
-            f"Successfully initiated "
-            f"{type(self.current_task).__name__} "
-            f"task: {str(self.current_task.id)[:8]}"
-        )
-
-    def _resume(self) -> None:
-        self.logger.info(f"Resuming mission: {self.current_mission.id}")
-        self.current_mission.status = MissionStatus.InProgress
-        self.current_mission.error_message = None
-        self.current_task.status = TaskStatus.InProgress
-
-        self.publish_mission_status()
-        self.publish_task_status(task=self.current_task)
-
-        resume_mission_response: ControlMissionResponse = (
-            self._make_control_mission_response()
-        )
-        self.queues.resume_mission.output.put(resume_mission_response)
-
-        self.robot.resume()
-
-    def _mission_finished(self) -> None:
-        fail_statuses: List[TaskStatus] = [
-            TaskStatus.Cancelled,
-            TaskStatus.Failed,
-        ]
-        partially_fail_statuses = fail_statuses + [TaskStatus.PartiallySuccessful]
-
-        if len(self.current_mission.tasks) == 0:
-            self.current_mission.status = MissionStatus.Successful
-        elif all(task.status in fail_statuses for task in self.current_mission.tasks):
-            self.current_mission.error_message = ErrorMessage(
-                error_reason=None,
-                error_description="The mission failed because all tasks in the mission "
-                "failed",
-            )
-            self.current_mission.status = MissionStatus.Failed
-        elif any(
-            task.status in partially_fail_statuses
-            for task in self.current_mission.tasks
-        ):
-            self.current_mission.status = MissionStatus.PartiallySuccessful
-        else:
-            self.current_mission.status = MissionStatus.Successful
-        self._finalize()
-
-    def _mission_started(self) -> None:
-        self.queues.start_mission.output.put(True)
-        self.logger.info(
-            f"Initialization successful. Starting new mission: "
-            f"{self.current_mission.id}"
-        )
-        self.log_mission_overview(mission=self.current_mission)
-
-        self.current_mission.status = MissionStatus.InProgress
-        self.publish_mission_status()
-        self.current_task = self.task_selector.next_task()
-        if self.current_task is None:
-            self._mission_finished()
-        else:
-            self.current_task.status = TaskStatus.InProgress
-            self.publish_task_status(task=self.current_task)
-
-    def _full_mission_finished(self) -> None:
-        self.current_task = None
-
-    def _mission_paused(self) -> None:
-        self.logger.info(f"Pausing mission: {self.current_mission.id}")
-        self.current_mission.status = MissionStatus.Paused
-        self.current_task.status = TaskStatus.Paused
-
-        paused_mission_response: ControlMissionResponse = (
-            self._make_control_mission_response()
-        )
-        self.queues.pause_mission.output.put(paused_mission_response)
-
-        self.publish_mission_status()
-        self.publish_task_status(task=self.current_task)
-
-        self.robot.pause()
-
-    def _initiate_failed(self) -> None:
-        self.current_task.status = TaskStatus.Failed
-        self.current_mission.status = MissionStatus.Failed
-        self.publish_task_status(task=self.current_task)
-        self._finalize()
-
-    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:
-            if task.status in [
-                TaskStatus.NotStarted,
-                TaskStatus.InProgress,
-                TaskStatus.Paused,
-            ]:
-                task.status = TaskStatus.Cancelled
-
-        stopped_mission_response: ControlMissionResponse = (
-            self._make_control_mission_response()
-        )
-        self.queues.stop_mission.output.put(stopped_mission_response)
-
-        self.publish_task_status(task=self.current_task)
-        self._finalize()
-
     #################################################################################
 
     def _finalize(self) -> None:
@@ -350,11 +229,7 @@ class StateMachine(object):
         self.reset_state_machine()
 
     def begin(self):
-        """Starts the state machine.
-
-        Transitions into idle state.
-
-        """
+        """Starts the state machine. Transitions into idle state."""
         self.to_idle()  # type: ignore
 
     def iterate_current_task(self):
@@ -366,6 +241,7 @@ class StateMachine(object):
             except TaskSelectorStop:
                 # Indicates that all tasks are finished
                 self.current_task = None
+            self.send_task_status()
 
     def update_state(self):
         """Updates the current state of the state machine."""
@@ -378,43 +254,78 @@ class StateMachine(object):
     def reset_state_machine(self) -> None:
         self.logger.info("Resetting state machine")
         self.current_task = None
+        self.send_task_status()
         self.current_mission = None
-        self.initial_pose = None
 
-    def start_mission(self, mission: Mission, initial_pose: Pose):
+    def start_mission(self, mission: Mission):
         """Starts a scheduled mission."""
         self.current_mission = mission
-        self.initial_pose = initial_pose
 
         self.task_selector.initialize(tasks=self.current_mission.tasks)
 
     def should_start_mission(self) -> Optional[StartMissionMessage]:
         try:
-            return self.queues.start_mission.input.get(block=False)
+            return self.queues.api_start_mission.input.get(block=False)
         except queue.Empty:
             return None
 
     def should_stop_mission(self) -> bool:
         try:
-            return self.queues.stop_mission.input.get(block=False)
+            return self.queues.api_stop_mission.input.get(block=False)
         except queue.Empty:
             return False
 
     def should_pause_mission(self) -> bool:
         try:
-            return self.queues.pause_mission.input.get(block=False)
+            return self.queues.api_pause_mission.input.get(block=False)
+        except queue.Empty:
+            return False
+
+    def get_task_status_event(self) -> Optional[TaskStatus]:
+        try:
+            return self.queues.robot_task_status.input.get(block=False)
+        except queue.Empty:
+            return None
+
+    def request_task_status(self, task_id: str) -> None:
+        self.queues.state_machine_task_status_request.input.put(task_id)
+
+    def get_mission_started_event(self) -> bool:
+        try:
+            return self.queues.robot_mission_started.input.get(block=False)
         except queue.Empty:
             return False
 
+    def get_mission_failed_event(self) -> Optional[ErrorMessage]:
+        try:
+            return self.queues.robot_mission_failed.input.get(block=False)
+        except queue.Empty:
+            return None
+
+    def get_task_failure_event(self) -> Optional[ErrorMessage]:
+        try:
+            return self.queues.robot_task_status_failed.input.get(block=False)
+        except queue.Empty:
+            return None
+
     def should_resume_mission(self) -> bool:
         try:
-            return self.queues.resume_mission.input.get(block=False)
+            return self.queues.api_resume_mission.input.get(block=False)
         except queue.Empty:
             return False
 
-    def send_state_status(self):
+    def get_robot_status(self) -> bool:
+        try:
+            return self.queues.robot_status.check()
+        except queue.Empty:
+            return False
+
+    def send_state_status(self) -> None:
         self.queues.state.update(self.current_state)
 
+    def send_task_status(self):
+        self.queues.state_machine_current_task.update(self.current_task)
+
     def publish_mission_status(self) -> None:
         if not self.mqtt_publisher:
             return
@@ -502,11 +413,11 @@ class StateMachine(object):
         else:
             return RobotStatus.Busy
 
-    def _log_state_transition(self, next_state):
+    def _log_state_transition(self, next_state) -> None:
         """Logs all state transitions that are not self-transitions."""
         self.transitions_list.append(next_state)
 
-    def log_mission_overview(self, mission: Mission):
+    def log_mission_overview(self, mission: Mission) -> None:
         """Log an overview of the tasks in a mission"""
         log_statements: List[str] = []
         for task in mission.tasks:
@@ -525,8 +436,8 @@ class StateMachine(object):
             task_status=self.current_task.status,
         )
 
-    def _queue_empty_response(self):
-        self.queues.stop_mission.output.put(
+    def _queue_empty_response(self) -> None:
+        self.queues.api_stop_mission.output.put(
             ControlMissionResponse(
                 mission_id="None",
                 mission_status="None",

isar/state_machine/states/blocked_protective_stop.py

@@ -4,12 +4,7 @@ from typing import TYPE_CHECKING, Optional
 
 from transitions import State
 
-from isar.config.settings import settings
-from isar.services.utilities.threaded_request import (
-    ThreadedRequest,
-    ThreadedRequestNotFinishedError,
-)
-from robot_interface.models.exceptions.robot_exceptions import RobotException
+from isar.services.utilities.threaded_request import ThreadedRequest
 from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
@@ -44,22 +39,15 @@ class BlockedProtectiveStop(State):
                     name="State Machine BlockedProtectiveStop Get Robot Status"
                 )
 
-            try:
-                robot_status: RobotStatus = self.robot_status_thread.get_output()
-            except ThreadedRequestNotFinishedError:
-                time.sleep(self.state_machine.sleep_time)
-                continue
+            robot_status = self.state_machine.get_robot_status()
 
-            except RobotException as e:
-                self.logger.error(
-                    f"Failed to get robot status because: {e.error_description}"
-                )
-
-            if robot_status != RobotStatus.BlockedProtectiveStop:
+            if robot_status == RobotStatus.Offline:
+                transition = self.state_machine.robot_turned_offline  # type: ignore
+                break
+            elif robot_status != RobotStatus.BlockedProtectiveStop:
                 transition = self.state_machine.robot_protective_stop_disengaged  # type: ignore
                 break
 
-            self.robot_status_thread = None
-            time.sleep(settings.ROBOT_API_STATUS_POLL_INTERVAL)
+            time.sleep(self.state_machine.sleep_time)
 
         transition()

isar/state_machine/states/idle.py

@@ -1,16 +1,11 @@
 import logging
 import time
-from typing import TYPE_CHECKING, Optional
+from typing import TYPE_CHECKING, Callable, Optional
 
 from transitions import State
 
 from isar.config.settings import settings
 from isar.models.communication.message import StartMissionMessage
-from isar.services.utilities.threaded_request import (
-    ThreadedRequest,
-    ThreadedRequestNotFinishedError,
-)
-from robot_interface.models.exceptions.robot_exceptions import RobotException
 from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
@@ -22,24 +17,16 @@ class Idle(State):
         super().__init__(name="idle", on_enter=self.start, on_exit=self.stop)
         self.state_machine: "StateMachine" = state_machine
         self.logger = logging.getLogger("state_machine")
-        self.robot_status_thread: Optional[ThreadedRequest] = None
         self.last_robot_status_poll_time: float = time.time()
-        self.status_checked_at_least_once: bool = False
 
     def start(self) -> None:
         self.state_machine.update_state()
         self._run()
 
     def stop(self) -> None:
-        if self.robot_status_thread:
-            self.robot_status_thread.wait_for_thread()
-        self.robot_status_thread = None
-        self.status_checked_at_least_once = False
+        return
 
     def _is_ready_to_poll_for_status(self) -> bool:
-        if not self.status_checked_at_least_once:
-            return True
-
         time_since_last_robot_status_poll = (
             time.time() - self.last_robot_status_poll_time
         )
@@ -48,49 +35,21 @@ class Idle(State):
         )
 
     def _run(self) -> None:
+        transition: Callable
         while True:
             if self.state_machine.should_stop_mission():
                 transition = self.state_machine.stop  # type: ignore
                 break
 
-            if self.status_checked_at_least_once:
-                start_mission: Optional[StartMissionMessage] = (
-                    self.state_machine.should_start_mission()
-                )
-                if start_mission:
-                    self.state_machine.start_mission(
-                        mission=start_mission.mission,
-                        initial_pose=start_mission.initial_pose,
-                    )
-                    transition = self.state_machine.mission_started  # type: ignore
-                    break
-                time.sleep(self.state_machine.sleep_time)
-
-            if not self._is_ready_to_poll_for_status():
-                continue
-
-            if not self.robot_status_thread:
-                self.robot_status_thread = ThreadedRequest(
-                    request_func=self.state_machine.robot.robot_status
-                )
-                self.robot_status_thread.start_thread(
-                    name="State Machine Offline Get Robot Status"
-                )
-
-            try:
-                robot_status: RobotStatus = self.robot_status_thread.get_output()
-                self.status_checked_at_least_once = True
-            except ThreadedRequestNotFinishedError:
-                time.sleep(self.state_machine.sleep_time)
-                continue
-
-            except RobotException as e:
-                self.logger.error(
-                    f"Failed to get robot status because: {e.error_description}"
-                )
-
-            self.last_robot_status_poll_time = time.time()
+            start_mission: Optional[StartMissionMessage] = (
+                self.state_machine.should_start_mission()
+            )
+            if start_mission:
+                self.state_machine.start_mission(mission=start_mission.mission)
+                transition = self.state_machine.request_mission_start  # type: ignore
+                break
 
+            robot_status = self.state_machine.get_robot_status()
             if robot_status == RobotStatus.Offline:
                 transition = self.state_machine.robot_turned_offline  # type: ignore
                 break
@@ -98,6 +57,5 @@ class Idle(State):
                 transition = self.state_machine.robot_protective_stop_engaged  # type: ignore
                 break
 
-            self.robot_status_thread = None
-
+            time.sleep(self.state_machine.sleep_time)
         transition()