Add return home

src/isar/apis/models/start_mission_definition.py

@@ -61,17 +61,13 @@ class StartMissionDefinition(BaseModel):
 
 def to_isar_mission(
     start_mission_definition: StartMissionDefinition,
-    return_pose: Optional[InputPose] = None,
 ) -> Mission:
     isar_tasks: List[TASKS] = []
 
     for task_definition in start_mission_definition.tasks:
         task: TASKS = to_isar_task(task_definition)
         isar_tasks.append(task)
 
-    if return_pose:
-        isar_tasks.append(ReturnToHome(pose=return_pose.to_alitra_pose()))
-
     if not isar_tasks:
         raise MissionPlannerError("Mission does not contain any valid tasks")
 
@@ -96,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"
@@ -166,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())}"

src/isar/apis/schedule/scheduling_controller.py

@@ -1,15 +1,12 @@
 import logging
 from http import HTTPStatus
-from typing import Optional
 
-from alitra import Pose
 from fastapi import Body, HTTPException, Path
 from injector import inject
 
 from isar.apis.models.models import (
     ControlMissionResponse,
     TaskResponse,
-    InputPose,
     StartMissionResponse,
 )
 from isar.apis.models.start_mission_definition import (
@@ -25,7 +22,6 @@
     TASKS,
     InspectionTask,
     MoveArm,
-    ReturnToHome,
 )
 
 
@@ -45,22 +41,13 @@ def start_mission_by_id(
             title="Mission ID",
             description="ID-number for predefined mission",
         ),
-        return_pose: Optional[InputPose] = Body(
-            default=None,
-            description="End pose of the mission. The robot return to the specified "
-            "pose after finishing all inspections",
-            embed=True,
-        ),
     ) -> StartMissionResponse:
         self.logger.info(f"Received request to start mission with id {mission_id}")
 
         state: States = self.scheduling_utilities.get_state()
         self.scheduling_utilities.verify_state_machine_ready_to_receive_mission(state)
 
         mission: Mission = self.scheduling_utilities.get_mission(mission_id)
-        if return_pose:
-            pose: Pose = return_pose.to_alitra_pose()
-            mission.tasks.append(ReturnToHome(pose=pose))
 
         self.scheduling_utilities.verify_robot_capable_of_mission(
             mission=mission, robot_capabilities=robot_settings.CAPABILITIES
@@ -80,12 +67,6 @@ def start_mission(
             title="Mission Definition",
             description="Description of the mission in json format",
         ),
-        return_pose: Optional[InputPose] = Body(
-            default=None,
-            description="End pose of the mission. The robot return to the specified "
-            "pose after finishing all inspections",
-            embed=True,
-        ),
     ) -> StartMissionResponse:
         self.logger.info("Received request to start new mission")
 
@@ -103,7 +84,7 @@ def start_mission(
 
         try:
             mission: Mission = to_isar_mission(
-                start_mission_definition=mission_definition, return_pose=return_pose
+                start_mission_definition=mission_definition
             )
         except MissionPlannerError as e:
             error_message = f"Bad Request - Cannot create ISAR mission: {e}"

src/isar/config/settings.py

@@ -82,6 +82,9 @@ def __init__(self) -> None:
     # 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")
 

src/isar/services/utilities/scheduling_utilities.py

@@ -126,12 +126,16 @@ def verify_state_machine_ready_to_receive_mission(self, state: States) -> bool:
         HTTPException 409 Conflict
             If state machine is not idle and therefore can not 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)
-
-        return True
+        if (
+            state == States.Docked
+            or state == States.Idle
+            or state == States.AwaitNextMission
+        ):
+            return True
+
+        error_message = f"Conflict - Robot is not idle - State: {state}"
+        self.logger.warning(error_message)
+        raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
 
     def start_mission(
         self,

src/isar/state_machine/state_machine.py

@@ -17,10 +17,13 @@
 )
 from isar.models.communication.message import StartMissionMessage
 from isar.models.communication.queues.queues import Queues
+from isar.state_machine.states.docked import Docked
 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.returning_home import ReturningHome
+from isar.state_machine.states.await_next_mission import AwaitNextMission
 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
@@ -30,7 +33,7 @@
 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.task import TASKS
+from robot_interface.models.mission.task import TASKS, ReturnToHome
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.telemetry.payloads import (
@@ -83,20 +86,26 @@ def __init__(
         # List of states
         self.stop_state: State = Stop(self)
         self.paused_state: State = Paused(self)
+        self.docked_state: State = Docked(self)
         self.idle_state: State = Idle(self)
         self.initialize_state: State = Initialize(self)
         self.monitor_state: State = Monitor(self)
+        self.returning_home_state: State = ReturningHome(self)
+        self.await_next_mission_state: State = AwaitNextMission(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)
 
         self.states: List[State] = [
             self.off_state,
+            self.docked_state,
             self.idle_state,
             self.initialize_state,
             self.initiate_state,
             self.monitor_state,
+            self.returning_home_state,
+            self.await_next_mission_state,
             self.stop_state,
             self.paused_state,
             self.offline_state,
@@ -126,25 +135,49 @@ def __init__(
                 {
                     "trigger": "stop",
                     "source": [
+                        self.docked_state,
                         self.idle_state,
                         self.initiate_state,
                         self.monitor_state,
+                        self.returning_home_state,
+                        self.await_next_mission_state,
                         self.paused_state,
                     ],
                     "dest": self.stop_state,
                 },
                 {
                     "trigger": "mission_finished",
                     "source": self.monitor_state,
-                    "dest": self.idle_state,
+                    "dest": self.await_next_mission_state,
                     "before": self._mission_finished,
                 },
                 {
                     "trigger": "mission_started",
-                    "source": self.idle_state,
+                    "source": [
+                        self.docked_state,
+                        self.idle_state,
+                        self.await_next_mission_state,
+                    ],
                     "dest": self.initialize_state,
                     "before": self._mission_started,
                 },
+                {
+                    "trigger": "return_home",
+                    "source": self.await_next_mission_state,
+                    "dest": self.initialize_state,
+                    "before": self._return_home,
+                },
+                {
+                    "trigger": "return_home_initiated",
+                    "source": self.initiate_state,
+                    "dest": self.returning_home_state,
+                },
+                {
+                    "trigger": "return_home_finished",
+                    "source": self.returning_home_state,
+                    "dest": self.idle_state,
+                    "before": self._return_home_finished,
+                },
                 {
                     "trigger": "initialization_successful",
                     "source": self.initialize_state,
@@ -185,6 +218,16 @@ def __init__(
                     "source": self.offline_state,
                     "dest": self.idle_state,
                 },
+                {
+                    "trigger": "robot_docked",
+                    "source": self.idle_state,
+                    "dest": self.docked_state,
+                },
+                {
+                    "trigger": "robot_undocked",
+                    "source": self.docked_state,
+                    "dest": self.idle_state,
+                },
                 {
                     "trigger": "robot_protective_stop_engaged",
                     "source": self.idle_state,
@@ -285,6 +328,22 @@ def _mission_started(self) -> None:
             self.current_task.status = TaskStatus.InProgress
             self.publish_task_status(task=self.current_task)
 
+    def _return_home(self) -> None:
+        self.start_mission(
+            Mission(
+                tasks=[ReturnToHome()],
+                name="Return Home",
+            )
+        )
+        self.log_mission_overview(mission=self.current_mission)
+        self.current_mission.status = MissionStatus.InProgress
+
+        self.current_task = self.task_selector.next_task()
+        self.current_task.status = TaskStatus.InProgress
+
+    def _return_home_finished(self) -> None:
+        self.reset_state_machine()
+
     def _full_mission_finished(self) -> None:
         self.current_task = None
 
@@ -485,7 +544,11 @@ def publish_status(self) -> None:
         )
 
     def _current_status(self) -> RobotStatus:
-        if self.current_state == States.Idle:
+        if (
+            self.current_state == States.Docked
+            or self.current_state == States.Idle
+            or self.current_state == States.AwaitNextMission
+        ):
             return RobotStatus.Available
         elif self.current_state == States.Offline:
             return RobotStatus.Offline

src/isar/state_machine/states/await_next_mission.py

@@ -0,0 +1,56 @@
+import logging
+import time
+from typing import TYPE_CHECKING, Optional
+
+from transitions import State
+
+from isar.config.settings import settings
+from isar.models.communication.message import StartMissionMessage
+
+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
+
+    def start(self) -> None:
+        self.state_machine.update_state()
+        self.entered_time = time.time()
+        self._run()
+
+    def stop(self) -> None:
+        pass
+
+    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.state_machine.should_stop_mission():
+                transition = self.state_machine.stop  # type: ignore
+                break
+
+            start_mission: Optional[StartMissionMessage] = (
+                self.state_machine.should_start_mission()
+            )
+            if start_mission:
+                self.state_machine.start_mission(mission=start_mission.mission)
+                transition = self.state_machine.mission_started  # type: ignore
+                break
+
+            if self._should_return_home():
+                transition = self.state_machine.return_home  # type: ignore
+                break
+
+            time.sleep(self.state_machine.sleep_time)
+
+        transition()