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

isar/services/utilities/scheduling_utilities.py

@@ -2,9 +2,8 @@ import logging
 from copy import deepcopy
 from http import HTTPStatus
 from queue import Empty
-from typing import Any, List, Optional, Set
+from typing import Any, List
 
-from alitra import Pose
 from fastapi import HTTPException
 from injector import inject
 from requests import HTTPError
@@ -17,11 +16,9 @@ from isar.mission_planner.mission_planner_interface import (
     MissionPlannerInterface,
 )
 from isar.models.communication.message import StartMissionMessage
+from isar.models.communication.queues.events import APIRequests, Events, SharedState
 from isar.models.communication.queues.queue_io import QueueIO
-from isar.models.communication.queues.queues import Queues
-from isar.models.communication.queues.queue_timeout_error import (
-    QueueTimeoutError,
-)
+from isar.models.communication.queues.queue_timeout_error import QueueTimeoutError
 from isar.services.utilities.queue_utilities import QueueUtilities
 from isar.state_machine.states_enum import States
 from robot_interface.models.mission.mission import Mission
@@ -36,11 +33,13 @@ class SchedulingUtilities:
     @inject
     def __init__(
         self,
-        queues: Queues,
+        events: Events,
+        shared_state: SharedState,
         mission_planner: MissionPlannerInterface,
         queue_timeout: int = settings.QUEUE_TIMEOUT,
     ):
-        self.queues: Queues = queues
+        self.api_events: APIRequests = events.api_requests
+        self.shared_state: SharedState = shared_state
         self.mission_planner: MissionPlannerInterface = mission_planner
         self.queue_timeout: int = queue_timeout
         self.logger = logging.getLogger("api")
@@ -54,7 +53,7 @@ class SchedulingUtilities:
             If the current state is not available on the queue
         """
         try:
-            return self.queues.state.check()
+            return self.shared_state.state.check()
         except Empty:
             error_message: str = (
                 "Internal Server Error - Current state of the state machine is unknown"
@@ -102,14 +101,11 @@ class SchedulingUtilities:
         HTTPException 400 Bad request
             If the robot is not capable of performing mission
         """
-        is_capable: bool = True
-        missing_capabilities: Set[str] = set()
-        for task in mission.tasks:
-            if task.type not in robot_capabilities:
-                is_capable = False
-                missing_capabilities.add(task.type)
-
-        if not is_capable:
+        missing_capabilities = {
+            task.type for task in mission.tasks if task.type not in robot_capabilities
+        }
+
+        if missing_capabilities:
             error_message = (
                 f"Bad Request - Robot is not capable of performing mission."
                 f" Missing functionalities: {missing_capabilities}."
@@ -120,7 +116,7 @@ class SchedulingUtilities:
                 detail=error_message,
             )
 
-        return is_capable
+        return True
 
     def verify_state_machine_ready_to_receive_mission(self, state: States) -> bool:
         """Verify that the state machine is idle and ready to receive a mission
@@ -130,18 +126,16 @@ class SchedulingUtilities:
         HTTPException 409 Conflict
             If state machine is not idle and therefore can not start a new mission
         """
-        is_state_machine_ready_to_receive_mission = state == States.Idle
-        if not is_state_machine_ready_to_receive_mission:
+        if state != States.Idle:
             error_message = f"Conflict - Robot is not idle - State: {state}"
             self.logger.warning(error_message)
             raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
 
-        return is_state_machine_ready_to_receive_mission
+        return True
 
     def start_mission(
         self,
         mission: Mission,
-        initial_pose: Optional[Pose],
     ) -> None:
         """Start mission
 
@@ -152,11 +146,8 @@ class SchedulingUtilities:
         """
         try:
             self._send_command(
-                StartMissionMessage(
-                    mission=deepcopy(mission),
-                    initial_pose=initial_pose,
-                ),
-                self.queues.start_mission,
+                StartMissionMessage(mission=deepcopy(mission)),
+                self.api_events.start_mission,
             )
         except QueueTimeoutError:
             error_message = "Internal Server Error - Failed to start mission in ISAR"
@@ -175,7 +166,7 @@ class SchedulingUtilities:
             If there is a timeout while communicating with the state machine
         """
         try:
-            return self._send_command(True, self.queues.pause_mission)
+            return self._send_command(True, self.api_events.pause_mission)
         except QueueTimeoutError:
             error_message = "Internal Server Error - Failed to pause mission"
             self.logger.error(error_message)
@@ -194,7 +185,7 @@ class SchedulingUtilities:
             If there is a timeout while communicating with the state machine
         """
         try:
-            return self._send_command(True, self.queues.resume_mission)
+            return self._send_command(True, self.api_events.resume_mission)
         except QueueTimeoutError:
             error_message = "Internal Server Error - Failed to resume mission"
             self.logger.error(error_message)
@@ -214,7 +205,7 @@ class SchedulingUtilities:
         """
         try:
             stop_mission_response: ControlMissionResponse = self._send_command(
-                True, self.queues.stop_mission
+                True, self.api_events.stop_mission
             )
         except QueueTimeoutError:
             error_message = "Internal Server Error - Failed to stop mission"

isar/state_machine/state_machine.py

@@ -1,11 +1,9 @@
 import json
 import logging
-import queue
 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
@@ -16,27 +14,43 @@ from isar.mission_planner.task_selector_interface import (
     TaskSelectorInterface,
     TaskSelectorStop,
 )
-from isar.models.communication.message import StartMissionMessage
-from isar.models.communication.queues.queues import Queues
+from isar.models.communication.queues.events import Events, SharedState
+from isar.models.communication.queues.queue_utils import update_shared_state
+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_cleanup,
+    trigger_stop_mission_event,
+)
+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
@@ -48,7 +62,8 @@ class StateMachine(object):
     @inject
     def __init__(
         self,
-        queues: Queues,
+        events: Events,
+        shared_state: SharedState,
         robot: RobotInterface,
         mqtt_publisher: MqttClientInterface,
         task_selector: TaskSelectorInterface,
@@ -60,8 +75,8 @@ class StateMachine(object):
 
         Parameters
         ----------
-        queues : Queues
-            Queues used for API communication.
+        events : Events
+            Events used for API and robot service communication.
         robot : RobotInterface
             Instance of robot interface.
         mqtt_publisher : MqttClientInterface
@@ -76,7 +91,8 @@ class StateMachine(object):
         """
         self.logger = logging.getLogger("state_machine")
 
-        self.queues: Queues = queues
+        self.events: Events = events
+        self.shared_state: SharedState = shared_state
         self.robot: RobotInterface = robot
         self.mqtt_publisher: Optional[MqttClientInterface] = mqtt_publisher
         self.task_selector: TaskSelectorInterface = task_selector
@@ -85,9 +101,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 +109,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 +124,68 @@ 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,
+                    "before": def_transition(self, trigger_stop_mission_event),
                 },
                 {
-                    "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_cleanup),
                 },
                 {
                     "trigger": "robot_turned_offline",
-                    "source": self.idle_state,
+                    "source": [self.idle_state, self.blocked_protective_stop],
                     "dest": self.offline_state,
                 },
                 {
@@ -188,7 +195,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 +211,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 +234,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,11 +246,12 @@ 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."""
         self.current_state = States(self.state)  # type: ignore
-        self.send_state_status()
+        update_shared_state(self.shared_state.state, self.current_state)
         self._log_state_transition(self.current_state)
         self.logger.info(f"State: {self.current_state}")
         self.publish_status()
@@ -378,42 +259,19 @@ 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)
-        except queue.Empty:
-            return None
-
-    def should_stop_mission(self) -> bool:
-        try:
-            return self.queues.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)
-        except queue.Empty:
-            return False
-
-    def should_resume_mission(self) -> bool:
-        try:
-            return self.queues.resume_mission.input.get(block=False)
-        except queue.Empty:
-            return False
-
-    def send_state_status(self):
-        self.queues.state.update(self.current_state)
+    def send_task_status(self):
+        update_shared_state(
+            self.shared_state.state_machine_current_task, self.current_task
+        )
 
     def publish_mission_status(self) -> None:
         if not self.mqtt_publisher:
@@ -502,11 +360,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 +383,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.events.api_requests.stop_mission.output.put(
             ControlMissionResponse(
                 mission_id="None",
                 mission_status="None",

isar/state_machine/states/blocked_protective_stop.py

@@ -4,12 +4,8 @@ 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.models.communication.queues.queue_utils import check_shared_state
+from isar.services.utilities.threaded_request import ThreadedRequest
 from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
@@ -24,42 +20,26 @@ class BlockedProtectiveStop(State):
         self.state_machine: "StateMachine" = state_machine
         self.logger = logging.getLogger("state_machine")
         self.robot_status_thread: Optional[ThreadedRequest] = None
+        self.shared_state = self.state_machine.shared_state
 
     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
+        return
 
     def _run(self) -> None:
         while True:
-            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 BlockedProtectiveStop Get Robot Status"
-                )
+            robot_status = check_shared_state(self.shared_state.robot_status)
 
-            try:
-                robot_status: RobotStatus = self.robot_status_thread.get_output()
-            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}"
-                )
-
-            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()