isar 1.26.4__py3-none-any.whl → 1.27.0__py3-none-any.whl

isar/state_machine/states/home.py

@@ -0,0 +1,87 @@
+import logging
+import time
+from queue import Queue
+from typing import TYPE_CHECKING, Optional
+
+from transitions import State
+
+from isar.models.communication.message import StartMissionMessage
+from isar.models.communication.queues.queue_io import QueueIO
+from isar.models.communication.queues.queue_utils import (
+    check_for_event,
+    check_shared_state,
+)
+from isar.models.communication.queues.status_queue import StatusQueue
+from robot_interface.models.mission.status import RobotStatus
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+class Home(State):
+    def __init__(self, state_machine: "StateMachine") -> None:
+        super().__init__(name="home", on_enter=self.start, on_exit=self.stop)
+        self.state_machine: "StateMachine" = state_machine
+        self.logger = logging.getLogger("state_machine")
+        self.events = self.state_machine.events
+        self.shared_state = self.state_machine.shared_state
+        self.signal_state_machine_to_stop = state_machine.signal_state_machine_to_stop
+
+    def start(self) -> None:
+        self.state_machine.update_state()
+        self._run()
+
+    def stop(self) -> None:
+        return
+
+    def _check_and_handle_start_mission_event(
+        self, event: Queue[StartMissionMessage]
+    ) -> bool:
+        start_mission: Optional[StartMissionMessage] = check_for_event(event)
+        if start_mission:
+            self.state_machine.start_mission(mission=start_mission.mission)
+            self.state_machine.request_mission_start()  # type: ignore
+            return True
+        return False
+
+    def _check_and_handle_return_home_event(self, event: QueueIO) -> bool:
+        if check_for_event(event.input):
+            event.output.put(True)
+            self.state_machine.request_return_home()  # type: ignore
+            return True
+        return False
+
+    def _check_and_handle_robot_status_event(
+        self, event: StatusQueue[RobotStatus]
+    ) -> bool:
+        robot_status: RobotStatus = check_shared_state(event)
+        if robot_status != RobotStatus.Home:
+            self.state_machine.robot_status_changed()  # type: ignore
+            return True
+
+        return False
+
+    def _run(self) -> None:
+        while True:
+            if self.signal_state_machine_to_stop.is_set():
+                self.logger.info(
+                    "Stopping state machine from %s state", self.__class__.__name__
+                )
+                break
+
+            if self._check_and_handle_start_mission_event(
+                self.events.api_requests.start_mission.input
+            ):
+                break
+
+            if self._check_and_handle_return_home_event(
+                self.events.api_requests.return_home
+            ):
+                break
+
+            if self._check_and_handle_robot_status_event(
+                self.shared_state.robot_status
+            ):
+                break
+
+            time.sleep(self.state_machine.sleep_time)

isar/state_machine/states/monitor.py

@@ -61,9 +61,8 @@ class Monitor(State):
         return False
 
     def _check_and_handle_mission_started_event(self, event: Queue) -> None:
-        if not self.state_machine.mission_ongoing:
-            if check_for_event(event):
-                self.state_machine.mission_ongoing = True
+        if check_for_event(event):
+            self.state_machine.mission_ongoing = True
 
     def _check_and_handle_mission_failed_event(self, event: Queue) -> bool:
         mission_failed: Optional[ErrorMessage] = check_for_event(event)
@@ -83,6 +82,9 @@ class Monitor(State):
         return False
 
     def _check_and_handle_task_status_failed_event(self, event: Queue) -> bool:
+        if not self.state_machine.mission_ongoing:
+            return False
+
         task_failure: Optional[ErrorMessage] = check_for_event(event)
         if task_failure is not None:
             self.awaiting_task_status = False
@@ -101,6 +103,9 @@ class Monitor(State):
         return False
 
     def _check_and_handle_task_status_event(self, event: Queue) -> bool:
+        if not self.state_machine.mission_ongoing:
+            return False
+
         status: Optional[TaskStatus] = check_for_event(event)
         if status is not None:
             self.awaiting_task_status = False

isar/state_machine/states/offline.py

@@ -37,11 +37,9 @@ class Offline(State):
             robot_status: RobotStatus = check_shared_state(
                 self.shared_state.robot_status
             )
-            if robot_status == RobotStatus.BlockedProtectiveStop:
-                transition = self.state_machine.robot_protective_stop_engaged  # type: ignore
-                break
-            elif robot_status != RobotStatus.Offline:
-                transition = self.state_machine.robot_turned_online  # type: ignore
+
+            if robot_status != RobotStatus.Offline:
+                transition = self.state_machine.robot_status_changed  # type: ignore
                 break
 
             time.sleep(self.state_machine.sleep_time)

isar/state_machine/states/returning_home.py

@@ -0,0 +1,186 @@
+import logging
+from queue import Queue
+from typing import TYPE_CHECKING, Optional
+
+from transitions import State
+
+from isar.models.communication.message import StartMissionMessage
+from isar.models.communication.queues.queue_utils import (
+    check_for_event,
+    check_for_event_without_consumption,
+    trigger_event,
+)
+from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
+from robot_interface.models.mission.status import TaskStatus
+from robot_interface.models.mission.task import Task
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+class ReturningHome(State):
+    def __init__(self, state_machine: "StateMachine") -> None:
+        super().__init__(name="returning_home", on_enter=self.start, on_exit=self.stop)
+        self.state_machine: "StateMachine" = state_machine
+
+        self.logger = logging.getLogger("state_machine")
+        self.events = self.state_machine.events
+
+        self.awaiting_task_status: bool = False
+        self.signal_state_machine_to_stop = state_machine.signal_state_machine_to_stop
+
+    def start(self) -> None:
+        self.state_machine.update_state()
+        self._run()
+
+    def stop(self) -> None:
+        self.state_machine.mission_ongoing = False
+        return
+
+    def _check_and_handle_stop_mission_event(self, event: Queue) -> bool:
+        if check_for_event(event):
+            self.state_machine.stop()  # type: ignore
+            return True
+        return False
+
+    def _check_and_handle_mission_started_event(self, event: Queue) -> bool:
+        if self.state_machine.mission_ongoing:
+            return False
+
+        if check_for_event(event):
+            self.state_machine.mission_ongoing = True
+            return False
+
+        return True
+
+    def _check_and_handle_mission_failed_event(self, event: Queue) -> bool:
+        mission_failed: Optional[ErrorMessage] = check_for_event(event)
+        if mission_failed is not None:
+            self.state_machine.logger.warning(
+                f"Failed to initiate mission "
+                f"{str(self.state_machine.current_mission.id)[:8]} because: "
+                f"{mission_failed.error_description}"
+            )
+            self.state_machine.current_mission.error_message = ErrorMessage(
+                error_reason=mission_failed.error_reason,
+                error_description=mission_failed.error_description,
+            )
+
+            self.state_machine.mission_failed_to_start()  # type: ignore
+            return True
+        return False
+
+    def _check_and_handle_task_status_failed_event(self, event: Queue) -> bool:
+        task_failure: Optional[ErrorMessage] = check_for_event(event)
+        if task_failure is not None:
+            self.awaiting_task_status = False
+            self.state_machine.current_task.error_message = task_failure
+            self.logger.error(
+                f"Monitoring task {self.state_machine.current_task.id[:8]} failed "
+                f"because: {task_failure.error_description}"
+            )
+            return self._handle_new_task_status(TaskStatus.Failed)
+        elif not self.awaiting_task_status:
+            trigger_event(
+                self.events.state_machine_events.task_status_request,
+                self.state_machine.current_task.id,
+            )
+            self.awaiting_task_status = True
+        return False
+
+    def _check_and_handle_task_status_event(self, event: Queue) -> bool:
+        status: Optional[TaskStatus] = check_for_event(event)
+        if status is not None:
+            self.awaiting_task_status = False
+            return self._handle_new_task_status(status)
+        elif not self.awaiting_task_status:
+            trigger_event(
+                self.events.state_machine_events.task_status_request,
+                self.state_machine.current_task.id,
+            )
+            self.awaiting_task_status = True
+        return False
+
+    def _handle_new_task_status(self, status: TaskStatus) -> bool:
+        if self.state_machine.current_task is None:
+            self.state_machine.iterate_current_task()
+
+        self.state_machine.current_task.status = status
+
+        if self.state_machine.current_task.is_finished():
+            self._report_task_status(self.state_machine.current_task)
+            self.state_machine.publish_task_status(task=self.state_machine.current_task)
+
+            self.state_machine.iterate_current_task()
+            if self.state_machine.current_task is None:
+                if status != TaskStatus.Successful:
+                    self.state_machine.return_home_failed()  # type: ignore
+                self.state_machine.returned_home()  # type: ignore
+                return True
+
+            # Report and update next task
+            self.state_machine.current_task.update_task_status()
+            self.state_machine.publish_task_status(task=self.state_machine.current_task)
+        return False
+
+    def _check_and_handle_start_mission_event(
+        self, event: Queue[StartMissionMessage]
+    ) -> bool:
+        if check_for_event_without_consumption(event):
+            self.state_machine.stop()  # type: ignore
+            return True
+
+        return False
+
+    def _run(self) -> None:
+        while True:
+            if self.signal_state_machine_to_stop.is_set():
+                self.logger.info(
+                    "Stopping state machine from %s state", self.__class__.__name__
+                )
+                break
+
+            if self._check_and_handle_stop_mission_event(
+                self.events.api_requests.stop_mission.input
+            ):
+                break
+
+            if self._check_and_handle_mission_started_event(
+                self.events.robot_service_events.mission_started
+            ):
+                continue
+
+            if self._check_and_handle_task_status_event(
+                self.events.robot_service_events.task_status_updated
+            ):
+                break
+
+            if self._check_and_handle_start_mission_event(
+                self.events.api_requests.start_mission.input
+            ):
+                break
+
+            if self._check_and_handle_mission_failed_event(
+                self.events.robot_service_events.mission_failed
+            ):
+                break
+
+            if self._check_and_handle_task_status_failed_event(
+                self.events.robot_service_events.task_status_failed
+            ):
+                break
+
+            if self._check_and_handle_task_status_event(
+                self.events.robot_service_events.task_status_updated
+            ):
+                break
+
+    def _report_task_status(self, task: Task) -> None:
+        if task.status == TaskStatus.Failed:
+            self.logger.warning(
+                f"Task: {str(task.id)[:8]} was reported as failed by the robot"
+            )
+        elif task.status == TaskStatus.Successful:
+            self.logger.info(
+                f"{type(task).__name__} task: {str(task.id)[:8]} completed"
+            )

isar/state_machine/states/{idle.py → robot_standing_still.py}

@@ -6,6 +6,7 @@ from typing import TYPE_CHECKING, Optional
 from transitions import State
 
 from isar.models.communication.message import StartMissionMessage
+from isar.models.communication.queues.queue_io import QueueIO
 from isar.models.communication.queues.queue_utils import (
     check_for_event,
     check_shared_state,
@@ -17,12 +18,13 @@ if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
 
-class Idle(State):
+class RobotStandingStill(State):
     def __init__(self, state_machine: "StateMachine") -> None:
-        super().__init__(name="idle", on_enter=self.start, on_exit=self.stop)
+        super().__init__(
+            name="robot_standing_still", on_enter=self.start, on_exit=self.stop
+        )
         self.state_machine: "StateMachine" = state_machine
         self.logger = logging.getLogger("state_machine")
-        self.last_robot_status_poll_time: float = time.time()
         self.events = self.state_machine.events
         self.shared_state = self.state_machine.shared_state
         self.signal_state_machine_to_stop = state_machine.signal_state_machine_to_stop
@@ -50,16 +52,21 @@ class Idle(State):
             return True
         return False
 
+    def _check_and_handle_return_home_event(self, event: QueueIO) -> bool:
+        if check_for_event(event.input):
+            event.output.put(True)
+            self.state_machine.request_return_home()  # type: ignore
+            return True
+        return False
+
     def _check_and_handle_robot_status_event(
         self, event: StatusQueue[RobotStatus]
     ) -> bool:
         robot_status: RobotStatus = check_shared_state(event)
-        if robot_status == RobotStatus.Offline:
-            self.state_machine.robot_turned_offline()  # type: ignore
-            return True
-        elif robot_status == RobotStatus.BlockedProtectiveStop:
-            self.state_machine.robot_protective_stop_engaged()  # type: ignore
+        if robot_status != RobotStatus.Available:
+            self.state_machine.robot_status_changed()  # type: ignore
             return True
+
         return False
 
     def _run(self) -> None:
@@ -80,6 +87,11 @@ class Idle(State):
             ):
                 break
 
+            if self._check_and_handle_return_home_event(
+                self.events.api_requests.return_home
+            ):
+                break
+
             if self._check_and_handle_robot_status_event(
                 self.shared_state.robot_status
             ):

isar/state_machine/states/{stop.py → stopping.py}

@@ -13,18 +13,23 @@ if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
 
-class Stop(State):
+class Stopping(State):
     def __init__(self, state_machine: "StateMachine") -> None:
-        super().__init__(name="stop", on_enter=self.start, on_exit=self.stop)
+        super().__init__(name="stopping", on_enter=self.start, on_exit=self.stop)
         self.state_machine: "StateMachine" = state_machine
         self.logger = logging.getLogger("state_machine")
         self.events = self.state_machine.events
         self.stop_thread: Optional[ThreadedRequest] = None
         self._count_number_retries: int = 0
         self.signal_state_machine_to_stop = state_machine.signal_state_machine_to_stop
+        self.stopping_return_home_mission: bool = False
 
     def start(self) -> None:
         self.state_machine.update_state()
+        if self.state_machine.current_mission is not None:
+            self.stopping_return_home_mission = (
+                self.state_machine.current_mission._is_return_to_home_mission()
+            )
         self._run()
 
     def stop(self) -> None:
@@ -32,18 +37,25 @@ class Stop(State):
             self.stop_thread.wait_for_thread()
         self.stop_thread = None
         self._count_number_retries = 0
+        self.stopping_return_home_mission = False
 
     def _check_and_handle_failed_stop(self, event: Queue[ErrorMessage]) -> bool:
         error_message: Optional[ErrorMessage] = check_for_event(event)
         if error_message is not None:
             self.logger.warning(error_message.error_description)
-            self.state_machine.mission_stopped()  # type: ignore
+            if self.stopping_return_home_mission:
+                self.state_machine.return_home_mission_stopped()  # type: ignore
+            else:
+                self.state_machine.mission_stopped()  # type: ignore
             return True
         return False
 
     def _check_and_handle_successful_stop(self, event: Queue[bool]) -> bool:
         if check_for_event(event):
-            self.state_machine.mission_stopped()  # type: ignore
+            if self.stopping_return_home_mission:
+                self.state_machine.return_home_mission_stopped()  # type: ignore
+            else:
+                self.state_machine.mission_stopped()  # type: ignore
             return True
         return False
 

isar/state_machine/states/unknown_status.py

@@ -0,0 +1,74 @@
+import logging
+import time
+from queue import Queue
+from typing import TYPE_CHECKING
+
+from transitions import State
+
+from isar.models.communication.queues.queue_utils import (
+    check_for_event,
+    check_shared_state,
+)
+from isar.models.communication.queues.status_queue import StatusQueue
+from robot_interface.models.mission.status import RobotStatus
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+class UnknownStatus(State):
+    def __init__(self, state_machine: "StateMachine") -> None:
+        super().__init__(name="unknown_status", on_enter=self.start, on_exit=self.stop)
+        self.state_machine: "StateMachine" = state_machine
+        self.logger = logging.getLogger("state_machine")
+        self.events = self.state_machine.events
+        self.shared_state = self.state_machine.shared_state
+        self.signal_state_machine_to_stop = state_machine.signal_state_machine_to_stop
+
+    def start(self) -> None:
+        self.state_machine.update_state()
+        self._run()
+
+    def stop(self) -> None:
+        return
+
+    def _check_and_handle_stop_mission_event(self, event: Queue) -> bool:
+        if check_for_event(event):
+            self.state_machine.stop()  # type: ignore
+            return True
+        return False
+
+    def _check_and_handle_robot_status_event(
+        self, event: StatusQueue[RobotStatus]
+    ) -> bool:
+        robot_status: RobotStatus = check_shared_state(event)
+        if (
+            robot_status == RobotStatus.Home
+            or robot_status == RobotStatus.Offline
+            or robot_status == RobotStatus.BlockedProtectiveStop
+            or robot_status == RobotStatus.Available
+        ):
+            self.state_machine.robot_status_changed()  # type: ignore
+            return True
+
+        return False
+
+    def _run(self) -> None:
+        while True:
+            if self.signal_state_machine_to_stop.is_set():
+                self.logger.info(
+                    "Stopping state machine from %s state", self.__class__.__name__
+                )
+                break
+
+            if self._check_and_handle_stop_mission_event(
+                self.events.api_requests.stop_mission.input
+            ):
+                break
+
+            if self._check_and_handle_robot_status_event(
+                self.shared_state.robot_status
+            ):
+                break
+
+            time.sleep(self.state_machine.sleep_time)

isar/state_machine/states_enum.py

@@ -2,13 +2,16 @@ from enum import Enum
 
 
 class States(str, Enum):
-    Off = "off"
-    Idle = "idle"
     Monitor = "monitor"
+    ReturningHome = "returning_home"
+    Stopping = "stopping"
     Paused = "paused"
-    Stop = "stop"
+    AwaitNextMission = "await_next_mission"
+    Home = "home"
+    RobotStandingStill = "robot_standing_still"
     Offline = "offline"
     BlockedProtectiveStop = "blocked_protective_stop"
+    UnknownStatus = "unknown_status"
 
     def __repr__(self):
         return self.value

isar/state_machine/transitions/functions/return_home.py

@@ -0,0 +1,38 @@
+from typing import TYPE_CHECKING
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+from typing import TYPE_CHECKING
+
+from robot_interface.models.mission.mission import Mission
+from robot_interface.models.mission.status import MissionStatus, TaskStatus
+from robot_interface.models.mission.task import ReturnToHome
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+def start_return_home_mission(state_machine: "StateMachine") -> bool:
+    state_machine.start_mission(
+        Mission(
+            tasks=[ReturnToHome()],
+            name="Return Home",
+        )
+    )
+    return True
+
+
+def set_return_home_status(state_machine: "StateMachine") -> bool:
+    state_machine.log_mission_overview(mission=state_machine.current_mission)
+    state_machine.current_mission.status = MissionStatus.InProgress
+
+    state_machine.current_task = state_machine.task_selector.next_task()
+    state_machine.current_task.status = TaskStatus.InProgress
+
+    return True
+
+
+def return_home_finished(state_machine: "StateMachine") -> bool:
+    state_machine.reset_state_machine()
+    return True

isar/state_machine/transitions/functions/robot_status.py

@@ -0,0 +1,27 @@
+from typing import TYPE_CHECKING
+
+from isar.models.communication.queues.queue_utils import check_shared_state
+from robot_interface.models.mission.status import RobotStatus
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+def is_offline(state_machine: "StateMachine") -> bool:
+    robot_status = check_shared_state(state_machine.shared_state.robot_status)
+    return robot_status == RobotStatus.Offline
+
+
+def is_available(state_machine: "StateMachine") -> bool:
+    robot_status = check_shared_state(state_machine.shared_state.robot_status)
+    return robot_status == RobotStatus.Available
+
+
+def is_home(state_machine: "StateMachine") -> bool:
+    robot_status = check_shared_state(state_machine.shared_state.robot_status)
+    return robot_status == RobotStatus.Home
+
+
+def is_blocked_protective_stop(state_machine: "StateMachine") -> bool:
+    robot_status = check_shared_state(state_machine.shared_state.robot_status)
+    return robot_status == RobotStatus.BlockedProtectiveStop

isar/state_machine/transitions/{start_mission.py → functions/start_mission.py}

@@ -61,7 +61,7 @@ def set_mission_to_in_progress(state_machine: "StateMachine") -> bool:
     return True
 
 
-def trigger_start_mission_or_task_event(state_machine: "StateMachine") -> bool:
+def trigger_start_mission_event(state_machine: "StateMachine") -> bool:
     state_machine.events.state_machine_events.start_mission.put(
         state_machine.current_mission
     )