Offline state refactor robot status

main.py

@@ -19,9 +19,6 @@
 from isar.services.service_connections.mqtt.robot_info_publisher import (
     RobotInfoPublisher,
 )
-from isar.services.service_connections.mqtt.robot_status_publisher import (
-    RobotStatusPublisher,
-)
 from isar.state_machine.state_machine import StateMachine, main
 from isar.storage.uploader import Uploader
 from robot_interface.robot_interface import RobotInterface
@@ -56,15 +53,6 @@
             target=mqtt_client.run, name="ISAR MQTT Client", daemon=True
         )
         threads.append(mqtt_thread)
-        robot_status_publisher: RobotStatusPublisher = RobotStatusPublisher(
-            mqtt_queue=queues.mqtt_queue, robot=robot, state_machine=state_machine
-        )
-        robot_status_thread: Thread = Thread(
-            target=robot_status_publisher.run,
-            name="ISAR Robot Status Publisher",
-            daemon=True,
-        )
-        threads.append(robot_status_thread)
 
         robot_info_publisher: RobotInfoPublisher = RobotInfoPublisher(
             mqtt_queue=queues.mqtt_queue

src/isar/config/settings.py

@@ -229,14 +229,13 @@ def __init__(self) -> None:
     DATA_CLASSIFICATION: str = Field(default="internal")
 
     # List of MQTT Topics
-    TOPIC_ISAR_STATE: str = Field(default="state", validate_default=True)
+    TOPIC_ISAR_STATUS: str = Field(default="status", validate_default=True)
     TOPIC_ISAR_MISSION: str = Field(default="mission", validate_default=True)
     TOPIC_ISAR_TASK: str = Field(default="task", validate_default=True)
     TOPIC_ISAR_STEP: str = Field(default="step", validate_default=True)
     TOPIC_ISAR_INSPECTION_RESULT: str = Field(
         default="inspection_result", validate_default=True
     )
-    TOPIC_ISAR_ROBOT_STATUS: str = Field(default="robot_status", validate_default=True)
     TOPIC_ISAR_ROBOT_INFO: str = Field(default="robot_info", validate_default=True)
     TOPIC_ISAR_ROBOT_HEARTBEAT: str = Field(
         default="robot_heartbeat", validate_default=True
@@ -284,11 +283,10 @@ def set_log_levels(cls, v: Any, info: ValidationInfo) -> dict:
         }
 
     @field_validator(
-        "TOPIC_ISAR_STATE",
+        "TOPIC_ISAR_STATUS",
         "TOPIC_ISAR_MISSION",
         "TOPIC_ISAR_TASK",
         "TOPIC_ISAR_STEP",
-        "TOPIC_ISAR_ROBOT_STATUS",
         "TOPIC_ISAR_ROBOT_INFO",
         "TOPIC_ISAR_ROBOT_HEARTBEAT",
         "TOPIC_ISAR_INSPECTION_RESULT",

src/isar/state_machine/state_machine.py

@@ -24,14 +24,20 @@
     Initiate,
     Monitor,
     Off,
+    Offline,
     Paused,
     Stop,
 )
 from isar.state_machine.states_enum import States
 from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
 from robot_interface.models.initialize.initialize_params import InitializeParams
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus, StepStatus, TaskStatus
+from robot_interface.models.mission.status import (
+    MissionStatus,
+    RobotStatus,
+    StepStatus,
+    TaskStatus,
+)
 from robot_interface.models.mission.step import Step
 from robot_interface.models.mission.task import Task
 from robot_interface.robot_interface import RobotInterface
@@ -88,6 +94,7 @@ def __init__(
         self.monitor_state: State = Monitor(self)
         self.initiate_state: State = Initiate(self)
         self.off_state: State = Off(self)
+        self.offline_state: State = Offline(self)
 
         self.states: List[State] = [
             self.off_state,
@@ -97,6 +104,7 @@ def __init__(
             self.monitor_state,
             self.stop_state,
             self.paused_state,
+            self.offline_state,
         ]
 
         self.machine = Machine(self, states=self.states, initial="off", queued=True)
@@ -194,6 +202,18 @@ def __init__(
                     "dest": self.idle_state,
                     "before": self._mission_stopped,
                 },
+                {
+                    "trigger": "robot_turned_offline",
+                    "source": [self.idle_state],
+                    "dest": self.offline_state,
+                    "before": self._offline,
+                },
+                {
+                    "trigger": "robot_turned_online",
+                    "source": self.offline_state,
+                    "dest": self.idle_state,
+                    "before": self._online,
+                },
             ]
         )
 
@@ -239,6 +259,12 @@ def _pause(self) -> None:
     def _off(self) -> None:
         return
 
+    def _offline(self) -> None:
+        return
+
+    def _online(self) -> None:
+        return
+
     def _resume(self) -> None:
         self.logger.info(f"Resuming mission: {self.current_mission.id}")
         self.current_mission.status = MissionStatus.InProgress
@@ -417,7 +443,7 @@ def update_state(self):
         self.send_state_status()
         self._log_state_transition(self.current_state)
         self.logger.info(f"State: {self.current_state}")
-        self.publish_state()
+        self.publish_status()
 
     def reset_state_machine(self) -> None:
         self.logger.info("Resetting state machine")
@@ -559,25 +585,33 @@ def publish_step_status(self, step: Step) -> None:
             retain=False,
         )
 
-    def publish_state(self) -> None:
+    def publish_status(self) -> None:
         if not self.mqtt_publisher:
             return
         payload: str = json.dumps(
             {
                 "isar_id": settings.ISAR_ID,
                 "robot_name": settings.ROBOT_NAME,
-                "state": self.current_state,
+                "status": self._current_status(),
                 "timestamp": datetime.utcnow(),
             },
             cls=EnhancedJSONEncoder,
         )
 
         self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_STATE,
+            topic=settings.TOPIC_ISAR_STATUS,
             payload=payload,
             retain=False,
         )
 
+    def _current_status(self) -> RobotStatus:
+        if self.current_state == States.Idle:
+            return RobotStatus.Available
+        elif self.current_state == States.Offline:
+            return RobotStatus.Offline
+        else:
+            return RobotStatus.Busy
+
     def _log_state_transition(self, next_state):
         """Logs all state transitions that are not self-transitions."""
         self.transitions_list.append(next_state)

src/isar/state_machine/states/__init__.py

@@ -3,5 +3,6 @@
 from .initiate import Initiate
 from .monitor import Monitor
 from .off import Off
+from .offline import Offline
 from .paused import Paused
 from .stop import Stop

src/isar/state_machine/states/idle.py

@@ -1,10 +1,17 @@
 import logging
 import time
-from typing import Optional, TYPE_CHECKING
+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.services.utilities.threaded_request import (
+    ThreadedRequest,
+    ThreadedRequestNotFinishedError,
+)
+from robot_interface.models.exceptions.robot_exceptions import RobotException
+from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
@@ -15,13 +22,17 @@ def __init__(self, state_machine: "StateMachine") -> None:
         super().__init__(name="idle", on_enter=self.start, on_exit=self.stop)
         self.state_machine: "StateMachine" = state_machine
         self.logger = logging.getLogger("state_machine")
+        self.robot_status_thread: Optional[ThreadedRequest] = None
+        self.last_robot_status_poll_time: float = time.time()
 
     def start(self) -> None:
         self.state_machine.update_state()
         self._run()
 
     def stop(self) -> None:
-        pass
+        if self.robot_status_thread:
+            self.robot_status_thread.wait_for_thread()
+        self.robot_status_thread = None
 
     def _run(self) -> None:
         while True:
@@ -37,4 +48,38 @@ def _run(self) -> None:
                 break
             time.sleep(self.state_machine.sleep_time)
 
+            time_from_last_robot_status_poll = (
+                time.time() - self.last_robot_status_poll_time
+            )
+            if (
+                time_from_last_robot_status_poll
+                < settings.ROBOT_API_STATUS_POLL_INTERVAL
+            ):
+                continue
+
+            if not self.robot_status_thread:
+                self.robot_status_thread = ThreadedRequest(
+                    request_func=self.state_machine.robot.robot_status
+                )
+                self.robot_status_thread.start_thread(
+                    name="State Machine Offline Get Robot Status"
+                )
+
+            try:
+                robot_status: RobotStatus = self.robot_status_thread.get_output()
+            except ThreadedRequestNotFinishedError:
+                time.sleep(self.state_machine.sleep_time)
+                continue
+
+            except (RobotException,) as e:
+                self.logger.error(
+                    f"Failed to get robot status because: {e.error_description}"
+                )
+
+            self.last_robot_status_poll_time = time.time()
+
+            if robot_status == RobotStatus.Offline:
+                transition = self.state_machine.robot_turned_offline  # type: ignore
+                break
+
         transition()