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

isar/apis/api.py

@@ -150,6 +150,31 @@ class API:
                 HTTPStatus.CONFLICT.value: {
                     "description": "Conflict - Invalid command in the current state",
                 },
+                HTTPStatus.BAD_REQUEST.value: {
+                    "description": "Bad request - Robot does not have the capabilities to perform the mission",
+                },
+                HTTPStatus.INTERNAL_SERVER_ERROR.value: {
+                    "description": "Internal Server Error - Current state of state machine unknown",
+                },
+            },
+        )
+        router.add_api_route(
+            path="/schedule/return-home",
+            endpoint=self.scheduling_controller.return_home,
+            methods=["POST"],
+            dependencies=[authentication_dependency],
+            summary="Start return home mission",
+            responses={
+                HTTPStatus.OK.value: {
+                    "description": "Return home mission succesfully started",
+                    "model": StartMissionResponse,
+                },
+                HTTPStatus.UNPROCESSABLE_ENTITY.value: {
+                    "description": "Invalid body - The JSON is incorrect",
+                },
+                HTTPStatus.CONFLICT.value: {
+                    "description": "Conflict - Invalid command in the current state",
+                },
                 HTTPStatus.INTERNAL_SERVER_ERROR.value: {
                     "description": "Internal Server Error - Current state of state machine unknown",
                 },

isar/apis/models/start_mission_definition.py

@@ -92,7 +92,7 @@ def to_isar_task(task_definition: StartMissionTaskDefinition) -> TASKS:
     if task_definition.type == TaskType.Inspection:
         return to_inspection_task(task_definition)
     elif task_definition.type == TaskType.ReturnToHome:
-        return create_return_to_home_task(task_definition)
+        return ReturnToHome()
     else:
         raise MissionPlannerError(
             f"Failed to create task: '{task_definition.type}' is not a valid"
@@ -162,11 +162,5 @@ def to_inspection_task(task_definition: StartMissionTaskDefinition) -> TASKS:
         )
 
 
-def create_return_to_home_task(
-    task_definition: StartMissionTaskDefinition,
-) -> ReturnToHome:
-    return ReturnToHome(pose=task_definition.pose.to_alitra_pose())
-
-
 def _build_mission_name() -> str:
     return f"{settings.PLANT_SHORT_NAME}{settings.ROBOT_NAME}{int(time.time())}"

isar/apis/schedule/scheduling_controller.py

@@ -98,6 +98,16 @@ class SchedulingController:
         self.scheduling_utilities.start_mission(mission=mission)
         return self._api_response(mission)
 
+    def return_home(self) -> None:
+        self.logger.info("Received request to return home")
+
+        state: States = self.scheduling_utilities.get_state()
+        self.scheduling_utilities.verify_state_machine_ready_to_receive_return_home_mission(
+            state
+        )
+
+        self.scheduling_utilities.return_home()
+
     def pause_mission(self) -> ControlMissionResponse:
         self.logger.info("Received request to pause current mission")
 
@@ -142,7 +152,13 @@ class SchedulingController:
 
         state: States = self.scheduling_utilities.get_state()
 
-        if state == States.Off:
+        if (
+            state == States.UnknownStatus
+            or state == States.Stopping
+            or state == States.BlockedProtectiveStop
+            or state == States.Offline
+            or state == States.Home
+        ):
             error_message = (
                 f"Conflict - Stop command received in invalid state - State: {state}"
             )

isar/config/settings.py

@@ -82,6 +82,9 @@ class Settings(BaseSettings):
     # FastAPI port
     API_PORT: int = Field(default=3000)
 
+    # Determines how long delay time should be allowed before returning home
+    RETURN_HOME_DELAY: int = Field(default=10)
+
     # Determines which mission planner module is used by ISAR
     MISSION_PLANNER: str = Field(default="local")
 
@@ -311,7 +314,7 @@ class RobotSettings(BaseSettings):
 
     # ISAR steps the robot is capable of performing
     # This should be set in the robot package settings.env file
-    CAPABILITIES: List[str] = Field(default=["return_to_home", "take_image"])
+    CAPABILITIES: List[str] = Field(default=["take_image"])
 
     # Model of the robot which ISAR is connected to
     # This should be set in the robot package settings.env file

isar/models/communication/queues/events.py

@@ -29,6 +29,7 @@ class APIRequests:
         self.stop_mission: QueueIO = QueueIO(input_size=1, output_size=1)
         self.pause_mission: QueueIO = QueueIO(input_size=1, output_size=1)
         self.resume_mission: QueueIO = QueueIO(input_size=1, output_size=1)
+        self.return_home: QueueIO = QueueIO(input_size=1, output_size=1)
 
 
 class StateMachineEvents:

isar/models/communication/queues/queue_utils.py

@@ -30,3 +30,9 @@ def check_for_event(queue: Queue[T]) -> Optional[T]:
         return queue.get(block=False)
     except Empty:
         return None
+
+
+def check_for_event_without_consumption(queue: Queue[T]) -> bool:
+    return (
+        queue.qsize() != 0
+    )  # Queue size is not reliable, but should be sufficient for this case

isar/modules.py

@@ -70,12 +70,19 @@ class ApplicationContainer(containers.DeclarativeContainer):
     )
 
     # Storage
-    local_storage = providers.Singleton(LocalStorage)
-    blob_storage = providers.Singleton(BlobStorage, keyvault=keyvault)
-    slimm_storage = providers.Singleton(
-        SlimmStorage, request_handler=providers.Singleton(RequestHandler)
-    )
-    storage_handlers = providers.List(local_storage, blob_storage, slimm_storage)
+    storage_handlers_temp = []
+    if settings.STORAGE_LOCAL_ENABLED:
+        local_storage = providers.Singleton(LocalStorage)
+        storage_handlers_temp.append(local_storage)
+    if settings.STORAGE_BLOB_ENABLED:
+        blob_storage = providers.Singleton(BlobStorage, keyvault=keyvault)
+        storage_handlers_temp.append(blob_storage)
+    if settings.STORAGE_SLIMM_ENABLED:
+        slimm_storage = providers.Singleton(
+            SlimmStorage, request_handler=providers.Singleton(RequestHandler)
+        )
+        storage_handlers_temp.append(slimm_storage)
+    storage_handlers = providers.List(*storage_handlers_temp)
 
     # Mqtt client
     mqtt_client = (

isar/robot/robot.py

@@ -11,11 +11,12 @@ from isar.models.communication.queues.events import (
     SharedState,
     StateMachineEvents,
 )
-from isar.models.communication.queues.queue_utils import check_for_event
+from isar.models.communication.queues.queue_utils import check_for_event, trigger_event
 from isar.robot.robot_start_mission import RobotStartMissionThread
 from isar.robot.robot_status import RobotStatusThread
 from isar.robot.robot_stop_mission import RobotStopMissionThread
 from isar.robot.robot_task_status import RobotTaskStatusThread
+from robot_interface.models.exceptions.robot_exceptions import ErrorMessage, ErrorReason
 from robot_interface.robot_interface import RobotInterface
 
 
@@ -94,6 +95,14 @@ class Robot(object):
                 self.stop_mission_thread_thread is not None
                 and self.start_mission_thread.is_alive()
             ):
+                error_description = "Received stop mission event while trying to start a mission. Aborting stop attempt."
+                error_message = ErrorMessage(
+                    error_reason=ErrorReason.RobotStillStartingMissionException,
+                    error_description=error_description,
+                )
+                trigger_event(
+                    self.robot_service_events.mission_failed_to_stop, error_message
+                )
                 return
             self.stop_mission_thread_thread = RobotStopMissionThread(
                 self.robot_service_events, self.robot, self.signal_thread_quitting

isar/robot/robot_status.py

@@ -5,6 +5,7 @@ from threading import Event, Thread
 from isar.config.settings import settings
 from isar.models.communication.queues.events import SharedState
 from isar.models.communication.queues.queue_utils import update_shared_state
+from robot_interface.models.exceptions.robot_exceptions import RobotException
 from robot_interface.robot_interface import RobotInterface
 
 
@@ -19,7 +20,9 @@ class RobotStatusThread(Thread):
         self.shared_state: SharedState = shared_state
         self.robot: RobotInterface = robot
         self.signal_thread_quitting: Event = signal_thread_quitting
-        self.last_robot_status_poll_time: float = time.time()
+        self.last_robot_status_poll_time: float = (
+            time.time() - settings.ROBOT_API_STATUS_POLL_INTERVAL
+        )
         Thread.__init__(self, name="Robot status thread")
 
     def stop(self) -> None:
@@ -40,9 +43,12 @@ class RobotStatusThread(Thread):
         while not self.signal_thread_quitting.wait(0.001):
             if not self._is_ready_to_poll_for_status():
                 continue
+            try:
+                robot_status = self.robot.robot_status()
 
-            robot_status = self.robot.robot_status()
-
-            update_shared_state(self.shared_state.robot_status, robot_status)
-            self.last_robot_status_poll_time = time.time()
+                update_shared_state(self.shared_state.robot_status, robot_status)
+                self.last_robot_status_poll_time = time.time()
+            except RobotException as e:
+                self.logger.error(f"Failed to retrieve robot status: {e}")
+                continue
         self.logger.info("Exiting robot status thread")

isar/services/utilities/scheduling_utilities.py

@@ -119,19 +119,47 @@ class SchedulingUtilities:
         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
+        """Verify that the state machine is ready to receive a mission
 
         Raises
         ------
         HTTPException 409 Conflict
-            If state machine is not idle and therefore can not start a new mission
+            If state machine is not home, robot standing still, awaiting next mission
+            or returning home and therefore cannot start a new 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)
+        if (
+            state == States.RobotStandingStill
+            or state == States.Home
+            or state == States.AwaitNextMission
+            or state == States.ReturningHome
+        ):
+            return True
 
-        return True
+        error_message = f"Conflict - Robot is not home, robot standing still, awaiting next mission or returning home - State: {state}"
+        self.logger.warning(error_message)
+        raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
+
+    def verify_state_machine_ready_to_receive_return_home_mission(
+        self, state: States
+    ) -> bool:
+        """Verify that the state machine is ready to receive a return home mission
+
+        Raises
+        ------
+        HTTPException 409 Conflict
+            If state machine is not home, robot standing still or awaiting next mission
+            and therefore cannot start a new return home mission
+        """
+        if (
+            state == States.RobotStandingStill
+            or state == States.Home
+            or state == States.AwaitNextMission
+        ):
+            return True
+
+        error_message = f"Conflict - Robot is not home, robot standing still or awaiting next mission - State: {state}"
+        self.logger.warning(error_message)
+        raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
 
     def start_mission(
         self,
@@ -157,6 +185,31 @@ class SchedulingUtilities:
             )
         self.logger.info("OK - Mission started in ISAR")
 
+    def return_home(
+        self,
+    ) -> None:
+        """Start return home mission
+
+        Raises
+        ------
+        HTTTPException 408 Request timeout
+            If there is a timeout while communicating with the state machine
+        """
+        try:
+            self._send_command(
+                True,
+                self.api_events.return_home,
+            )
+        except QueueTimeoutError:
+            error_message = (
+                "Internal Server Error - Failed to start return home mission in ISAR"
+            )
+            self.logger.error(error_message)
+            raise HTTPException(
+                status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
+            )
+        self.logger.info("OK - Return home mission started in ISAR")
+
     def pause_mission(self) -> ControlMissionResponse:
         """Pause mission
 

isar/state_machine/state_machine.py

@@ -17,32 +17,20 @@ from isar.mission_planner.task_selector_interface import (
 )
 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.await_next_mission import AwaitNextMission
 from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
-from isar.state_machine.states.idle import Idle
+from isar.state_machine.states.home import Home
 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.paused import Paused
-from isar.state_machine.states.stop import Stop
+from isar.state_machine.states.returning_home import ReturningHome
+from isar.state_machine.states.robot_standing_still import RobotStandingStill
+from isar.state_machine.states.stopping import Stopping
+from isar.state_machine.states.unknown_status import UnknownStatus
 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 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.mission.mission import Mission
 from robot_interface.models.mission.status import RobotStatus, TaskStatus
@@ -101,114 +89,49 @@ class StateMachine(object):
         self.signal_state_machine_to_stop: Event = Event()
 
         # List of states
-        self.stop_state: State = Stop(self)
-        self.paused_state: State = Paused(self)
-        self.idle_state: State = Idle(self)
+        # States running mission
         self.monitor_state: State = Monitor(self)
-        self.off_state: State = Off(self)
+        self.returning_home_state: State = ReturningHome(self)
+        self.stopping_state: State = Stopping(self)
+        self.paused_state: State = Paused(self)
+
+        # States Waiting for mission
+        self.await_next_mission_state: State = AwaitNextMission(self)
+        self.home_state: State = Home(self)
+        self.robot_standing_still_state: State = RobotStandingStill(self)
+
+        # Status states
         self.offline_state: State = Offline(self)
-        self.blocked_protective_stop: State = BlockedProtectiveStop(self)
+        self.blocked_protective_stopping_state: State = BlockedProtectiveStop(self)
+
+        # Error and special status states
+        self.unknown_status_state: State = UnknownStatus(self)
 
         self.states: List[State] = [
-            self.off_state,
-            self.idle_state,
             self.monitor_state,
-            self.stop_state,
+            self.returning_home_state,
+            self.stopping_state,
             self.paused_state,
+            self.await_next_mission_state,
+            self.home_state,
+            self.robot_standing_still_state,
             self.offline_state,
-            self.blocked_protective_stop,
+            self.blocked_protective_stopping_state,
+            self.unknown_status_state,
         ]
 
-        self.machine = Machine(self, states=self.states, initial="off", queued=True)
-        self.machine.add_transitions(
-            [
-                {
-                    "trigger": "start_machine",
-                    "source": self.off_state,
-                    "dest": self.idle_state,
-                },
-                {
-                    "trigger": "pause",
-                    "source": self.monitor_state,
-                    "dest": self.paused_state,
-                    "before": def_transition(self, pause_mission),
-                },
-                {
-                    "trigger": "stop",
-                    "source": [
-                        self.idle_state,
-                        self.monitor_state,
-                        self.paused_state,
-                    ],
-                    "dest": self.stop_state,
-                    "before": def_transition(self, trigger_stop_mission_event),
-                },
-                {
-                    "trigger": "request_mission_start",
-                    "source": self.idle_state,
-                    "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": "request_mission_start",
-                    "source": self.idle_state,
-                    "dest": self.idle_state,
-                },
-                {
-                    "trigger": "mission_failed_to_start",
-                    "source": self.monitor_state,
-                    "dest": self.idle_state,
-                    "before": def_transition(self, report_failed_mission_and_finalize),
-                },
-                {
-                    "trigger": "resume",
-                    "source": self.paused_state,
-                    "dest": self.monitor_state,
-                    "before": def_transition(self, resume_mission),
-                },
-                {
-                    "trigger": "mission_finished",
-                    "source": self.monitor_state,
-                    "dest": self.idle_state,
-                    "before": def_transition(self, finish_mission),
-                },
-                {
-                    "trigger": "mission_stopped",
-                    "source": self.stop_state,
-                    "dest": self.idle_state,
-                    "before": def_transition(self, stop_mission_cleanup),
-                },
-                {
-                    "trigger": "robot_turned_offline",
-                    "source": [self.idle_state, self.blocked_protective_stop],
-                    "dest": self.offline_state,
-                },
-                {
-                    "trigger": "robot_turned_online",
-                    "source": self.offline_state,
-                    "dest": self.idle_state,
-                },
-                {
-                    "trigger": "robot_protective_stop_engaged",
-                    "source": [self.idle_state, self.offline_state],
-                    "dest": self.blocked_protective_stop,
-                },
-                {
-                    "trigger": "robot_protective_stop_disengaged",
-                    "source": self.blocked_protective_stop,
-                    "dest": self.idle_state,
-                },
-            ]
+        self.machine = Machine(
+            self, states=self.states, initial="unknown_status", queued=True
         )
 
+        self.transitions: List[dict] = []
+
+        self.transitions.extend(get_mission_transitions(self))
+        self.transitions.extend(get_return_home_transitions(self))
+        self.transitions.extend(get_robot_status_transitions(self))
+
+        self.machine.add_transitions(self.transitions)
+
         self.stop_robot_attempts_limit: int = stop_robot_attempts_limit
         self.sleep_time: float = sleep_time
 
@@ -237,8 +160,8 @@ class StateMachine(object):
         self.reset_state_machine()
 
     def begin(self):
-        """Starts the state machine. Transitions into idle state."""
-        self.to_idle()  # type: ignore
+        """Starts the state machine. Transitions into unknown status state."""
+        self.robot_status_changed()  # type: ignore
 
     def terminate(self):
         self.logger.info("Stopping state machine")
@@ -358,8 +281,15 @@ class StateMachine(object):
         )
 
     def _current_status(self) -> RobotStatus:
-        if self.current_state == States.Idle:
+        if (
+            self.current_state == States.RobotStandingStill
+            or self.current_state == States.AwaitNextMission
+        ):
             return RobotStatus.Available
+        elif self.current_state == States.Home:
+            return RobotStatus.Home
+        elif self.current_state == States.ReturningHome:
+            return RobotStatus.ReturningHome
         elif self.current_state == States.Offline:
             return RobotStatus.Offline
         elif self.current_state == States.BlockedProtectiveStop:

isar/state_machine/states/await_next_mission.py

@@ -0,0 +1,92 @@
+import logging
+import time
+from queue import Queue
+from typing import TYPE_CHECKING, Optional
+
+from transitions import State
+
+from isar.config.settings import settings
+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
+
+if TYPE_CHECKING:
+    from isar.state_machine.state_machine import StateMachine
+
+
+class AwaitNextMission(State):
+    def __init__(self, state_machine: "StateMachine") -> None:
+        super().__init__(
+            name="await_next_mission", on_enter=self.start, on_exit=self.stop
+        )
+        self.state_machine: "StateMachine" = state_machine
+        self.logger = logging.getLogger("state_machine")
+        self.entered_time: float = time.time()
+        self.return_home_delay: float = settings.RETURN_HOME_DELAY
+        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.entered_time = time.time()
+        self._run()
+
+    def stop(self) -> None:
+        pass
+
+    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_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 _should_return_home(self) -> bool:
+        time_since_entered = time.time() - self.entered_time
+        return time_since_entered > self.return_home_delay
+
+    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_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._should_return_home():
+                self.state_machine.request_return_home()  # type: ignore
+                break
+
+            time.sleep(self.state_machine.sleep_time)

isar/state_machine/states/blocked_protective_stop.py

@@ -41,11 +41,8 @@ class BlockedProtectiveStop(State):
             robot_status: RobotStatus = check_shared_state(
                 self.shared_state.robot_status
             )
-            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
+            if robot_status != RobotStatus.BlockedProtectiveStop:
+                transition = self.state_machine.robot_status_changed  # type: ignore
                 break
 
             time.sleep(self.state_machine.sleep_time)