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,14 +1,11 @@
 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,
-    InputPose,
     StartMissionResponse,
     TaskResponse,
 )
@@ -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/robot/robot_status.py

@@ -17,7 +17,9 @@ def __init__(
         self.queues: Queues = queues
         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", daemon=True)
 
     def stop(self) -> None:
@@ -40,7 +42,6 @@ def run(self):
                 continue
 
             robot_status = self.robot.robot_status()
-
             update_shared_state(self.queues.robot_status, robot_status)
             self.last_robot_status_poll_time = time.time()
         self.logger.info("Exiting robot status thread")

src/isar/services/utilities/scheduling_utilities.py

@@ -122,14 +122,20 @@ def verify_state_machine_ready_to_receive_mission(self, state: States) -> bool:
         Raises
         ------
         HTTPException 409 Conflict
-            If state machine is not idle and therefore can not start a new mission
+            If state machine is not docked, idle or awaiting next mission
+            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)
-
-        return True
+        if (
+            state == States.Docked
+            or state == States.Idle
+            or state == States.AwaitNextMission
+            or state == States.ReturningHome
+        ):
+            return True
+
+        error_message = f"Conflict - Robot is not docked, idle or awaiting next mission - State: {state}"
+        self.logger.warning(error_message)
+        raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
 
     def start_mission(
         self,