isar 1.31.0__py3-none-any.whl → 1.32.0__py3-none-any.whl

isar/services/utilities/scheduling_utilities.py

@@ -1,8 +1,7 @@
 import logging
 from copy import deepcopy
 from http import HTTPStatus
-from queue import Empty
-from typing import Any, List
+from typing import List, TypeVar
 
 from fastapi import HTTPException
 from requests import HTTPError
@@ -14,15 +13,20 @@ from isar.mission_planner.mission_planner_interface import (
     MissionPlannerError,
     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.queue_timeout_error import QueueTimeoutError
-from isar.services.utilities.queue_utilities import QueueUtilities
+from isar.models.events import (
+    APIEvent,
+    APIRequests,
+    Events,
+    EventTimeoutError,
+    SharedState,
+)
 from isar.state_machine.states_enum import States
 from robot_interface.models.mission.mission import Mission
 from robot_interface.models.mission.status import MissionStatus
 
+T1 = TypeVar("T1")
+T2 = TypeVar("T2")
+
 
 class SchedulingUtilities:
     """
@@ -51,9 +55,8 @@ class SchedulingUtilities:
         HTTPException 500 Internal Server Error
             If the current state is not available on the queue
         """
-        try:
-            return self.shared_state.state.check()
-        except Empty:
+        current_state = self.shared_state.state.check()
+        if current_state is None:
             error_message: str = (
                 "Internal Server Error - Current state of the state machine is unknown"
             )
@@ -61,6 +64,7 @@ class SchedulingUtilities:
             raise HTTPException(
                 status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
             )
+        return current_state
 
     def get_mission(self, mission_id: str) -> Mission:
         """Get the mission with mission_id from the current mission planner
@@ -173,10 +177,10 @@ class SchedulingUtilities:
         """
         try:
             self._send_command(
-                StartMissionMessage(mission=deepcopy(mission)),
+                deepcopy(mission),
                 self.api_events.start_mission,
             )
-        except QueueTimeoutError:
+        except EventTimeoutError:
             error_message = "Internal Server Error - Failed to start mission in ISAR"
             self.logger.error(error_message)
             raise HTTPException(
@@ -199,7 +203,7 @@ class SchedulingUtilities:
                 True,
                 self.api_events.return_home,
             )
-        except QueueTimeoutError:
+        except EventTimeoutError:
             error_message = (
                 "Internal Server Error - Failed to start return home mission in ISAR"
             )
@@ -221,7 +225,7 @@ class SchedulingUtilities:
             response = self._send_command(True, self.api_events.pause_mission)
             self.logger.info("OK - Mission successfully paused")
             return response
-        except QueueTimeoutError:
+        except EventTimeoutError:
             error_message = "Internal Server Error - Failed to pause mission"
             self.logger.error(error_message)
             raise HTTPException(
@@ -240,7 +244,7 @@ class SchedulingUtilities:
             response = self._send_command(True, self.api_events.resume_mission)
             self.logger.info("OK - Mission successfully resumed")
             return response
-        except QueueTimeoutError:
+        except EventTimeoutError:
             error_message = "Internal Server Error - Failed to resume mission"
             self.logger.error(error_message)
             raise HTTPException(
@@ -277,7 +281,7 @@ class SchedulingUtilities:
                 raise HTTPException(
                     status_code=HTTPStatus.CONFLICT, detail=error_message
                 )
-        except QueueTimeoutError:
+        except EventTimeoutError:
             error_message = "Internal Server Error - Failed to stop mission"
             self.logger.error(error_message)
             raise HTTPException(
@@ -286,14 +290,32 @@ class SchedulingUtilities:
         self.logger.info("OK - Mission successfully stopped")
         return stop_mission_response
 
-    def _send_command(self, input: Any, queueio: QueueIO) -> Any:
-        queueio.input.put(input)
+    def release_intervention_needed(self) -> None:
+        """Release intervention needed state
+
+        Raises
+        ------
+        HTTPException 500 Internal Server Error
+            If the intervention needed state could not be released
+        """
         try:
-            return QueueUtilities.check_queue(
-                queueio.output,
-                self.queue_timeout,
+            self._send_command(True, self.api_events.release_intervention_needed)
+            self.logger.info("OK - Intervention needed state released")
+        except EventTimeoutError:
+            error_message = (
+                "Internal Server Error - Failed to release intervention needed state"
             )
-        except QueueTimeoutError as e:
-            QueueUtilities.clear_queue(queueio.input)
+            self.logger.error(error_message)
+            raise HTTPException(
+                status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
+            )
+
+    def _send_command(self, input: T1, api_event: APIEvent[T1, T2]) -> T2:
+        api_event.input.trigger_event(input)
+        try:
+            return api_event.output.consume_event(timeout=self.queue_timeout)
+        except EventTimeoutError as e:
+            self.logger.error("Queue timed out")
+            api_event.input.clear_event()
             self.logger.error("No output received for command to state machine")
             raise e

isar/state_machine/state_machine.py

@@ -3,7 +3,7 @@ import logging
 from collections import deque
 from datetime import datetime, timezone
 from threading import Event
-from typing import Deque, List, Optional
+from typing import Deque, List, Optional, Tuple
 
 from transitions import Machine
 from transitions.core import State
@@ -14,11 +14,11 @@ from isar.mission_planner.task_selector_interface import (
     TaskSelectorInterface,
     TaskSelectorStop,
 )
-from isar.models.communication.queues.events import Events, SharedState
-from isar.models.communication.queues.queue_utils import update_shared_state
+from isar.models.events import Events, SharedState
 from isar.state_machine.states.await_next_mission import AwaitNextMission
 from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
 from isar.state_machine.states.home import Home
+from isar.state_machine.states.intervention_needed import InterventionNeeded
 from isar.state_machine.states.monitor import Monitor
 from isar.state_machine.states.offline import Offline
 from isar.state_machine.states.paused import Paused
@@ -30,13 +30,19 @@ from isar.state_machine.states_enum import States
 from isar.state_machine.transitions.mission import get_mission_transitions
 from isar.state_machine.transitions.return_home import get_return_home_transitions
 from isar.state_machine.transitions.robot_status import get_robot_status_transitions
-from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    RobotException,
+    RobotRetrieveInspectionException,
+)
+from robot_interface.models.inspection.inspection import Inspection
 from robot_interface.models.mission.mission import Mission
 from robot_interface.models.mission.status import RobotStatus, TaskStatus
-from robot_interface.models.mission.task import TASKS
+from robot_interface.models.mission.task import TASKS, InspectionTask, Task
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.telemetry.payloads import (
+    InterventionNeededPayload,
     MissionPayload,
     RobotStatusPayload,
     TaskPayload,
@@ -97,6 +103,7 @@ class StateMachine(object):
         self.await_next_mission_state: State = AwaitNextMission(self)
         self.home_state: State = Home(self)
         self.robot_standing_still_state: State = RobotStandingStill(self)
+        self.intervention_needed_state: State = InterventionNeeded(self)
 
         # Status states
         self.offline_state: State = Offline(self)
@@ -116,6 +123,7 @@ class StateMachine(object):
             self.offline_state,
             self.blocked_protective_stopping_state,
             self.unknown_status_state,
+            self.intervention_needed_state,
         ]
 
         self.machine = Machine(
@@ -140,6 +148,8 @@ class StateMachine(object):
 
         self.current_state: State = States(self.state)  # type: ignore
 
+        self.awaiting_task_status: bool = False
+
         self.transitions_log_length: int = transitions_log_length
         self.transitions_list: Deque[States] = deque([], self.transitions_log_length)
 
@@ -182,7 +192,7 @@ class StateMachine(object):
     def update_state(self):
         """Updates the current state of the state machine."""
         self.current_state = States(self.state)  # type: ignore
-        update_shared_state(self.shared_state.state, self.current_state)
+        self.shared_state.state.update(self.current_state)
         self._log_state_transition(self.current_state)
         self.logger.info("State: %s", self.current_state)
         self.publish_status()
@@ -201,9 +211,21 @@ class StateMachine(object):
         self.task_selector.initialize(tasks=self.current_mission.tasks)
 
     def send_task_status(self):
-        update_shared_state(
-            self.shared_state.state_machine_current_task, self.current_task
-        )
+        self.shared_state.state_machine_current_task.update(self.current_task)
+
+    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"
+            )
+        else:
+            self.logger.info(
+                f"Task: {str(task.id)[:8]} was reported as task.status by the robot"
+            )
 
     def publish_mission_status(self) -> None:
         if not self.mqtt_publisher:
@@ -264,6 +286,25 @@ class StateMachine(object):
             retain=True,
         )
 
+    def publish_intervention_needed(self, error_message: str) -> None:
+        """Publishes the intervention needed message to the MQTT Broker"""
+        if not self.mqtt_publisher:
+            return
+
+        payload: InterventionNeededPayload = InterventionNeededPayload(
+            isar_id=settings.ISAR_ID,
+            robot_name=settings.ROBOT_NAME,
+            reason=error_message,
+            timestamp=datetime.now(timezone.utc),
+        )
+
+        self.mqtt_publisher.publish(
+            topic=settings.TOPIC_ISAR_INTERVENTION_NEEDED,
+            payload=json.dumps(payload, cls=EnhancedJSONEncoder),
+            qos=1,
+            retain=True,
+        )
+
     def publish_status(self) -> None:
         if not self.mqtt_publisher:
             return
@@ -296,6 +337,8 @@ class StateMachine(object):
             return RobotStatus.Offline
         elif self.current_state == States.BlockedProtectiveStop:
             return RobotStatus.BlockedProtectiveStop
+        elif self.current_state == States.InterventionNeeded:
+            return RobotStatus.InterventionNeeded
         else:
             return RobotStatus.Busy
 
@@ -334,6 +377,58 @@ class StateMachine(object):
             )
         )
 
+    def should_upload_inspections(self) -> bool:
+        if settings.UPLOAD_INSPECTIONS_ASYNC:
+            return False
+
+        return (
+            self.current_task.is_finished()
+            and self.current_task.status == TaskStatus.Successful
+            and isinstance(self.current_task, InspectionTask)
+        )
+
+    def queue_inspections_for_upload(
+        self, mission: Mission, current_task: InspectionTask, logger: logging.Logger
+    ) -> None:
+        try:
+            inspection: Inspection = self.robot.get_inspection(task=current_task)
+            if current_task.inspection_id != inspection.id:
+                logger.warning(
+                    f"The inspection_id of task ({current_task.inspection_id}) "
+                    f"and result ({inspection.id}) is not matching. "
+                    f"This may lead to confusions when accessing the inspection later"
+                )
+
+        except (RobotRetrieveInspectionException, RobotException) as e:
+            error_message: ErrorMessage = ErrorMessage(
+                error_reason=e.error_reason, error_description=e.error_description
+            )
+            self.current_task.error_message = error_message
+            logger.error(
+                f"Failed to retrieve inspections because: {e.error_description}"
+            )
+            return
+
+        except Exception as e:
+            logger.error(
+                f"Failed to retrieve inspections because of unexpected error: {e}"
+            )
+            return
+
+        if not inspection:
+            logger.warning(
+                f"No inspection result data retrieved for task {str(current_task.id)[:8]}"
+            )
+
+        inspection.metadata.tag_id = current_task.tag_id
+
+        message: Tuple[Inspection, Mission] = (
+            inspection,
+            mission,
+        )
+        self.events.upload_queue.put(message)
+        logger.info(f"Inspection result: {str(inspection.id)[:8]} queued for upload")
+
 
 def main(state_machine: StateMachine):
     """Starts a state machine instance."""

isar/state_machine/states/await_next_mission.py

@@ -1,20 +1,55 @@
-from typing import TYPE_CHECKING
+from typing import TYPE_CHECKING, List
 
-from transitions import State
+from isar.config.settings import settings
+from isar.eventhandlers.eventhandler import (
+    EventHandlerBase,
+    EventHandlerMapping,
+    TimeoutHandlerMapping,
+)
+from isar.state_machine.utils.common_event_handlers import (
+    return_home_event_handler,
+    start_mission_event_handler,
+    stop_mission_event_handler,
+)
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
-from isar.state_machine.generic_states.idle import Idle, IdleStates
 
+class AwaitNextMission(EventHandlerBase):
 
-class AwaitNextMission(State, Idle):
-    def __init__(self, state_machine: "StateMachine") -> None:
-        State.__init__(
-            self, name="await_next_mission", on_enter=self.start, on_exit=self.stop
-        )
-        Idle.__init__(
-            self,
+    def __init__(self, state_machine: "StateMachine"):
+        events = state_machine.events
+
+        event_handlers: List[EventHandlerMapping] = [
+            EventHandlerMapping(
+                name="start_mission_event",
+                event=events.api_requests.start_mission.input,
+                handler=lambda event: start_mission_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="return_home_event",
+                event=events.api_requests.return_home.input,
+                handler=lambda event: return_home_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="stop_mission_event",
+                event=events.api_requests.return_home.input,
+                handler=lambda event: stop_mission_event_handler(state_machine, event),
+            ),
+        ]
+
+        timers: List[TimeoutHandlerMapping] = [
+            TimeoutHandlerMapping(
+                name="should_return_home_timer",
+                timeout_in_seconds=settings.RETURN_HOME_DELAY,
+                handler=lambda: state_machine.request_return_home,  # type: ignore
+            )
+        ]
+
+        super().__init__(
+            state_name="await_next_mission",
             state_machine=state_machine,
-            state=IdleStates.AwaitNextMission,
+            event_handler_mappings=event_handlers,
+            timers=timers,
         )

isar/state_machine/states/blocked_protective_stop.py

@@ -1,24 +1,33 @@
-from typing import TYPE_CHECKING
+from typing import TYPE_CHECKING, List
 
-from transitions import State
-
-from isar.state_machine.generic_states.robot_unavailable import (
-    RobotUnavailable,
-    RobotUnavailableStates,
-)
+from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
+from isar.models.events import Event
+from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
 
-class BlockedProtectiveStop(State, RobotUnavailable):
-    def __init__(self, state_machine: "StateMachine") -> None:
-        State.__init__(
-            self, name="blocked_protective_stop", on_enter=self.start, on_exit=self.stop
-        )
+class BlockedProtectiveStop(EventHandlerBase):
+
+    def __init__(self, state_machine: "StateMachine"):
+        shared_state = state_machine.shared_state
+
+        def _robot_status_event_handler(event: Event[RobotStatus]):
+            robot_status: RobotStatus = event.check()
+            if robot_status != RobotStatus.BlockedProtectiveStop:
+                return state_machine.robot_status_changed  # type: ignore
+            return None
 
-        RobotUnavailable.__init__(
-            self,
+        event_handlers: List[EventHandlerMapping] = [
+            EventHandlerMapping(
+                name="robot_status_event",
+                event=shared_state.robot_status,
+                handler=_robot_status_event_handler,
+            ),
+        ]
+        super().__init__(
+            state_name="blocked_protective_stop",
             state_machine=state_machine,
-            state=RobotUnavailableStates.BlockedProtectiveStop,
+            event_handler_mappings=event_handlers,
         )

isar/state_machine/states/home.py

@@ -1,19 +1,50 @@
-from typing import TYPE_CHECKING
+from typing import TYPE_CHECKING, List
 
-from transitions import State
+from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
+from isar.state_machine.utils.common_event_handlers import (
+    return_home_event_handler,
+    robot_status_event_handler,
+    start_mission_event_handler,
+    stop_mission_event_handler,
+)
+from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
-from isar.state_machine.generic_states.idle import Idle, IdleStates
 
+class Home(EventHandlerBase):
 
-class Home(State, Idle):
-    def __init__(self, state_machine: "StateMachine") -> None:
-        State.__init__(self, name="home", on_enter=self.start, on_exit=self.stop)
+    def __init__(self, state_machine: "StateMachine"):
+        events = state_machine.events
+        shared_state = state_machine.shared_state
 
-        Idle.__init__(
-            self,
+        event_handlers: List[EventHandlerMapping] = [
+            EventHandlerMapping(
+                name="start_mission_event",
+                event=events.api_requests.start_mission.input,
+                handler=lambda event: start_mission_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="return_home_event",
+                event=events.api_requests.return_home.input,
+                handler=lambda event: return_home_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="stop_mission_event",
+                event=events.api_requests.return_home.input,
+                handler=lambda event: stop_mission_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="robot_status_event",
+                event=shared_state.robot_status,
+                handler=lambda event: robot_status_event_handler(
+                    state_machine, RobotStatus.Home, event
+                ),
+            ),
+        ]
+        super().__init__(
+            state_name="home",
             state_machine=state_machine,
-            state=IdleStates.Home,
+            event_handler_mappings=event_handlers,
         )

isar/state_machine/states/intervention_needed.py

@@ -0,0 +1,43 @@
+from collections.abc import Callable
+from typing import TYPE_CHECKING, List, Optional
+
+from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
+from isar.models.events import Event
+from isar.state_machine.utils.common_event_handlers import return_home_event_handler
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+class InterventionNeeded(EventHandlerBase):
+
+    def __init__(self, state_machine: "StateMachine"):
+        events = state_machine.events
+
+        def release_intervention_needed_handler(
+            event: Event[bool],
+        ) -> Optional[Callable]:
+            if event.consume_event():
+                state_machine.events.api_requests.release_intervention_needed.output.trigger_event(
+                    True
+                )
+                return state_machine.release_intervention_needed  # type: ignore
+            return None
+
+        event_handlers: List[EventHandlerMapping] = [
+            EventHandlerMapping(
+                name="return_home_event",
+                event=events.api_requests.return_home.input,
+                handler=lambda event: return_home_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="release_intervention_needed_event",
+                event=events.api_requests.release_intervention_needed.input,
+                handler=release_intervention_needed_handler,
+            ),
+        ]
+        super().__init__(
+            state_name="intervention_needed",
+            state_machine=state_machine,
+            event_handler_mappings=event_handlers,
+        )

isar/state_machine/states/monitor.py

@@ -1,22 +1,93 @@
-from typing import TYPE_CHECKING
+import logging
+from copy import deepcopy
+from typing import TYPE_CHECKING, Callable, List, Optional
 
-from transitions import State
-
-from isar.state_machine.generic_states.ongoing_mission import (
-    OngoingMission,
-    OngoingMissionStates,
+from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
+from isar.models.events import Event
+from isar.services.utilities.threaded_request import ThreadedRequest
+from isar.state_machine.utils.common_event_handlers import (
+    mission_failed_event_handler,
+    mission_started_event_handler,
+    stop_mission_event_handler,
+    task_status_event_handler,
+    task_status_failed_event_handler,
 )
+from robot_interface.models.mission.status import TaskStatus
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
 
-class Monitor(State, OngoingMission):
-    def __init__(self, state_machine: "StateMachine") -> None:
-        State.__init__(self, name="monitor", on_enter=self.start, on_exit=self.stop)
+class Monitor(EventHandlerBase):
+
+    def __init__(self, state_machine: "StateMachine"):
+        logger = logging.getLogger("state_machine")
+        events = state_machine.events
+
+        def _pause_mission_event_handler(event: Event[bool]) -> Optional[Callable]:
+            if event.consume_event():
+                return state_machine.pause  # type: ignore
+            return None
+
+        def _handle_task_completed(task_status: TaskStatus):
+            if state_machine.should_upload_inspections():
+                get_inspection_thread = ThreadedRequest(
+                    state_machine.queue_inspections_for_upload
+                )
+                get_inspection_thread.start_thread(
+                    deepcopy(state_machine.current_mission),
+                    deepcopy(state_machine.current_task),
+                    logger,
+                    name="State Machine Get Inspections",
+                )
+
+            state_machine.iterate_current_task()
+            if state_machine.current_task is None:
+                return state_machine.mission_finished  # type: ignore
+            return None
 
-        OngoingMission.__init__(
-            self,
+        event_handlers: List[EventHandlerMapping] = [
+            EventHandlerMapping(
+                name="stop_mission_event",
+                event=events.api_requests.stop_mission.input,
+                handler=lambda event: stop_mission_event_handler(state_machine, event),
+            ),
+            EventHandlerMapping(
+                name="pause_mission_event",
+                event=events.api_requests.pause_mission.input,
+                handler=_pause_mission_event_handler,
+            ),
+            EventHandlerMapping(
+                name="mission_started_event",
+                event=events.robot_service_events.mission_started,
+                handler=lambda event: mission_started_event_handler(
+                    state_machine, event
+                ),
+            ),
+            EventHandlerMapping(
+                name="mission_failed_event",
+                event=events.robot_service_events.mission_failed,
+                handler=lambda event: mission_failed_event_handler(
+                    state_machine, event
+                ),
+            ),
+            EventHandlerMapping(
+                name="task_status_failed_event",
+                event=events.robot_service_events.task_status_failed,
+                handler=lambda event: task_status_failed_event_handler(
+                    state_machine, _handle_task_completed, event
+                ),
+            ),
+            EventHandlerMapping(
+                name="task_status_event",
+                event=events.robot_service_events.task_status_updated,
+                handler=lambda event: task_status_event_handler(
+                    state_machine, _handle_task_completed, event
+                ),
+            ),
+        ]
+        super().__init__(
+            state_name="monitor",
             state_machine=state_machine,
-            state=OngoingMissionStates.Monitor,
+            event_handler_mappings=event_handlers,
         )