Simulate missions in separate thread

Author: andchiind

src/isar_robot/config/settings.py

@@ -1,4 +1,5 @@
 from importlib.resources import as_file, files
+
 from pydantic import Field
 from pydantic_settings import BaseSettings, SettingsConfigDict
 
@@ -13,15 +14,24 @@ def __init__(self) -> None:
             env_file = None
         super().__init__(_env_file=env_file)
 
-    TASK_DURATION_IN_SECONDS: float = Field(default=5.0)
+    MISSION_SIMULATION_TASK_DURATION: float = Field(default=5.0)
     INITIATE_MISSION_DURATION_IN_SECONDS: float = Field(default=0.1)
-    SHOULD_FAIL_NORMAL_TASK: bool = Field(default=False)
-    SHOULD_FAIL_RETURN_TO_HOME_TASK: bool = Field(default=False)
     SHOULD_HAVE_RANDOM_BATTERY_LEVEL: bool = Field(default=False)
     ROBOT_POSE_PUBLISH_INTERVAL: float = Field(default=5)
     ROBOT_BATTERY_PUBLISH_INTERVAL: float = Field(default=2)
     ROBOT_OBSTACLE_STATUS_PUBLISH_INTERVAL: float = Field(default=10)
     ROBOT_PRESSURE_PUBLISH_INTERVAL: float = Field(default=20)
+    MISSION_SIMULATION_TIME_TO_START: float = Field(default=5.0)
+    MISSION_SIMULATION_TIME_TO_STOP: float = Field(default=5.0)
+
+    # This is the time from the last task finishing to the mission finishing
+    MISSION_SIMULATION_MISSION_COMPLETION_DELAY: float = Field(default=2.0)
+    MISSION_SIMULATION_SHOULD_FAIL_RETURN_TO_HOME_TASK: bool = Field(default=False)
+    MISSION_SIMULATION_SHOULD_FAIL_NORMAL_TASK: bool = Field(default=False)
+    MISSION_SIMULATION_TASK_FAILURE_PROBABILITY: float = Field(default=0.05)
+
+    # This will cause delay between 0 and 5 seconds
+    MISSION_SIMULATION_API_DELAY_MODIFIER: float = Field(default=5.0)
 
     model_config = SettingsConfigDict(
         env_prefix="ROBOT_",

src/isar_robot/robotinterface.py

@@ -1,83 +1,70 @@
 import logging
-import time
 from datetime import datetime, timezone
 from logging import Logger
 from queue import Queue
 from threading import Thread
 from typing import Callable, List, Optional
 
+from robot_interface.models.exceptions.robot_exceptions import (
+    RobotCommunicationException,
+)
 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.status import MissionStatus, RobotStatus, TaskStatus
 from robot_interface.models.mission.task import (
     InspectionTask,
     RecordAudio,
-    ReturnToHome,
     TakeCO2Measurement,
     TakeImage,
     TakeThermalImage,
     TakeThermalVideo,
     TakeVideo,
-    Task,
 )
 from robot_interface.models.robots.media import MediaConfig
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttTelemetryPublisher
 
 from isar_robot import inspections, telemetry
 from isar_robot.config.settings import settings
+from isar_robot.simulation import MissionSimulation
 
 
 class Robot(RobotInterface):
     def __init__(self) -> None:
         self.telemetry = telemetry.Telemetry()
         self.logger: Logger = logging.getLogger("isar_robot")
-        self.current_mission: Optional[Mission] = None
-        self.current_task: Optional[Task] = None
         self.last_task_completion_time: datetime = datetime.now(timezone.utc)
         self.robot_is_home: bool = False
+        self.mission_simulation: Optional[MissionSimulation] = None
 
     def initiate_mission(self, mission: Mission) -> None:
-        time.sleep(settings.INITIATE_MISSION_DURATION_IN_SECONDS)
-        self.current_mission = mission
-        self.current_task_ix = 0
-        self.current_task = mission.tasks[self.current_task_ix]
-        self.task_len = len(mission.tasks)
-        self.last_task_completion_time = datetime.now(timezone.utc)
+        if self.mission_simulation and not self.mission_simulation.mission_done:
+            raise RobotCommunicationException(
+                error_description="Could not start mission as one is already running"
+            )
+        elif self.mission_simulation:
+            self.mission_simulation.join()
+        self.mission_simulation = MissionSimulation(mission)
+        self.mission_simulation.start()
         self.robot_is_home = False
 
     def task_status(self, task_id: str) -> TaskStatus:
-        now: datetime = datetime.now(timezone.utc)
-        if (
-            now - self.last_task_completion_time
-        ).total_seconds() < settings.TASK_DURATION_IN_SECONDS:
-            return TaskStatus.InProgress
-        self.last_task_completion_time = now
-
-        next_task: Task = None
-        if self.current_mission:
-            if self.current_task_ix < self.task_len - 1:
-                self.current_task_ix = self.current_task_ix + 1
-                next_task = self.current_mission.tasks[self.current_task_ix]
-
-        # This only happens for last task in mission
-        if isinstance(self.current_task, ReturnToHome):
-            self.current_task = None
-            if settings.SHOULD_FAIL_RETURN_TO_HOME_TASK:
-                return TaskStatus.Failed
-            self.robot_is_home = True
-            return TaskStatus.Successful
+        if not self.mission_simulation:
+            raise RobotCommunicationException(
+                error_description="Could not get task status as no mission is running"
+            )
 
-        if next_task:
-            self.current_task = next_task
-        else:
-            self.current_task = None
-        if settings.SHOULD_FAIL_NORMAL_TASK:
-            return TaskStatus.Failed
-        return TaskStatus.Successful
+        status = self.mission_simulation.task_status(task_id)
+        if status == TaskStatus.Successful and self.mission_simulation.is_return_home:
+            self.robot_is_home = True
+        return status
 
     def stop(self) -> None:
-        return
+        if not self.mission_simulation:
+            raise RobotCommunicationException(
+                error_description="Attempted to stop non-existent mission"
+            )
+        self.mission_simulation.stop_mission()
 
     def get_inspection(self, task: InspectionTask) -> Inspection:
         if type(task) in [TakeImage, TakeThermalImage]:
@@ -174,15 +161,29 @@ def get_telemetry_publishers(
         return publisher_threads
 
     def robot_status(self) -> RobotStatus:
+        if self.mission_simulation and self.mission_simulation.is_alive():
+            mission_status: MissionStatus = self.mission_simulation.mission_status()
+            if mission_status == MissionStatus.Paused:
+                return RobotStatus.Paused
+            elif mission_status in [MissionStatus.InProgress, MissionStatus.NotStarted]:
+                return RobotStatus.Busy
         if self.robot_is_home:
             return RobotStatus.Home
         return RobotStatus.Available
 
     def pause(self) -> None:
-        return
+        if not self.mission_simulation:
+            raise RobotCommunicationException(
+                error_description="Attempted to pause non-existent mission"
+            )
+        self.mission_simulation.pause_mission()
 
     def resume(self) -> None:
-        return
+        if not self.mission_simulation:
+            raise RobotCommunicationException(
+                error_description="Attempted to resume non-existent mission"
+            )
+        self.mission_simulation.resume_mission()
 
     def generate_media_config(self) -> Optional[MediaConfig]:
         return None

src/isar_robot/simulation.py

@@ -0,0 +1,143 @@
+import logging
+import random
+import time
+from threading import Event, Thread
+
+from robot_interface.models.exceptions.robot_exceptions import (
+    RobotCommunicationException,
+    RobotTaskStatusException,
+)
+from robot_interface.models.mission.mission import Mission
+from robot_interface.models.mission.status import MissionStatus, TaskStatus
+from robot_interface.models.mission.task import ReturnToHome
+
+from isar_robot.config.settings import settings
+
+
+class MissionSimulation(Thread):
+    def __init__(
+        self,
+        mission: Mission,
+    ):
+        self.logger = logging.getLogger("isar robot mission simulation")
+        self.mission = mission
+        self.task_index = 0
+        self.n_tasks = len(mission.tasks)
+        self.robot_is_home = False
+        self.task_statuses = list(
+            map(lambda _: TaskStatus.NotStarted, self.mission.tasks)
+        )
+        self.task_id_mapping = {}
+        for i, task in enumerate(self.mission.tasks):
+            self.task_id_mapping[task.id] = i
+
+        self.is_return_home: bool = len(self.mission.tasks) == 1 and isinstance(
+            self.mission.tasks[0], ReturnToHome
+        )
+        self.task_failure_probability: float = (
+            settings.MISSION_SIMULATION_TASK_FAILURE_PROBABILITY
+        )
+        self.api_delay_modifier: float = settings.MISSION_SIMULATION_API_DELAY_MODIFIER
+
+        self.mission_done: bool = False
+        self.all_tasks_done: bool = False
+
+        self.signal_resume_mission: Event = Event()
+        self.signal_resume_mission.set()
+        self.signal_stop_mission: Event = Event()
+        Thread.__init__(self, name="Mission simulation thread")
+
+    def stop(self) -> None:
+        return
+
+    def _simulate_api_call_delay(self):
+        time.sleep(random.random() * self.api_delay_modifier)
+
+    def pause_mission(self):
+        if self.mission_done:
+            raise RobotCommunicationException(
+                error_description="Could not pause non-existent mission"
+            )
+        self.signal_resume_mission.clear()
+
+    def resume_mission(self):
+        if self.mission_done:
+            raise RobotCommunicationException(
+                error_description="Could not resume non-existent mission"
+            )
+        self.signal_resume_mission.set()
+
+    def stop_mission(self):
+        if self.mission_done:
+            raise RobotCommunicationException(
+                error_description="Could not stop non-existent mission"
+            )
+        time.sleep(settings.MISSION_SIMULATION_TIME_TO_STOP)
+        self.signal_stop_mission.set()
+
+    def task_status(self, task_id: str):
+        task_index = self.task_id_mapping[task_id]
+        if task_index < 0 or task_index > self.n_tasks - 1:
+            raise RobotTaskStatusException(
+                error_description="Task ID did not match any ongoing tasks"
+            )
+        return self.task_statuses[task_index]
+
+    def mission_status(self):
+        if not self.signal_resume_mission.wait(0):
+            return MissionStatus.Paused
+        if all(map(lambda status: status == TaskStatus.Successful, self.task_statuses)):
+            return MissionStatus.Successful
+        if all(map(lambda status: status == TaskStatus.NotStarted, self.task_statuses)):
+            return MissionStatus.NotStarted
+        if any(
+            map(
+                lambda status: status in [TaskStatus.InProgress, TaskStatus.NotStarted],
+                self.task_statuses,
+            )
+        ):
+            return MissionStatus.InProgress
+        if all(map(lambda status: status == TaskStatus.Failed, self.task_statuses)):
+            return MissionStatus.Failed
+        if any(map(lambda status: status == TaskStatus.Cancelled, self.task_statuses)):
+            return MissionStatus.Cancelled
+        if any(map(lambda status: status == TaskStatus.Failed, self.task_statuses)):
+            return MissionStatus.PartiallySuccessful
+        raise RobotTaskStatusException("Unhandled task status detected")
+
+    def _complete_task(self, task_status: TaskStatus):
+        if self.task_index < self.n_tasks:
+            self.task_statuses[self.task_index] = task_status
+            self.task_index = self.task_index + 1
+        if self.task_index >= self.n_tasks:
+            self.all_tasks_done = True
+        else:
+            self.task_statuses[self.task_index] = TaskStatus.InProgress
+
+    def run(self):
+        time.sleep(settings.MISSION_SIMULATION_TIME_TO_START)
+
+        if self.signal_stop_mission.is_set():
+            return
+
+        thread_check_interval = settings.MISSION_SIMULATION_TASK_DURATION
+        while not self.signal_stop_mission.wait(thread_check_interval):
+
+            self.signal_resume_mission.wait()
+
+            if (
+                self.is_return_home
+                and not settings.MISSION_SIMULATION_SHOULD_FAIL_RETURN_TO_HOME_TASK
+            ) or not settings.MISSION_SIMULATION_SHOULD_FAIL_NORMAL_TASK:
+                self._complete_task(TaskStatus.Successful)
+            else:
+                if random.random() > self.task_failure_probability:
+                    self._complete_task(TaskStatus.Failed)
+                else:
+                    self._complete_task(TaskStatus.Successful)
+            if self.all_tasks_done:
+                break
+
+        time.sleep(settings.MISSION_SIMULATION_MISSION_COMPLETION_DELAY)
+        self.mission_done = True
+        self.logger.info("Exiting mission simulation thread")

src/isar_robot/telemetry.py

@@ -1,11 +1,9 @@
 import json
 import random
 from datetime import datetime, timezone
+from typing import Optional
 
 from alitra import Frame, Orientation, Pose, Position
-
-from isar_robot.config.settings import settings
-
 from robot_interface.models.robots.battery_state import BatteryState
 from robot_interface.telemetry.payloads import (
     TelemetryBatteryPayload,
@@ -15,7 +13,7 @@
 )
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
 
-from typing import Optional
+from isar_robot.config.settings import settings
 
 
 def _get_pressure_level() -> float: