isar 1.20.2__py3-none-any.whl → 1.34.9__py3-none-any.whl

isar/state_machine/state_machine.py

@@ -1,589 +1,277 @@
 import json
 import logging
-import queue
 from collections import deque
-from datetime import UTC, datetime
+from datetime import datetime, timezone
+from threading import Event
 from typing import Deque, List, Optional
 
-from alitra import Pose
-from injector import inject
 from transitions import Machine
 from transitions.core import State
 
-from isar.apis.models.models import ControlMissionResponse
 from isar.config.settings import settings
-from isar.mission_planner.task_selector_interface import (
-    TaskSelectorInterface,
-    TaskSelectorStop,
+from isar.models.events import Events, SharedState
+from isar.models.status import IsarStatus
+from isar.services.service_connections.persistent_memory import (
+    NoSuchRobotException,
+    create_persistent_robot_state,
+    read_persistent_robot_state_is_maintenance_mode,
 )
-from isar.models.communication.message import StartMissionMessage
-from isar.models.communication.queues.queues import Queues
-from isar.state_machine.states import (
-    Idle,
-    Initialize,
-    Initiate,
-    Monitor,
-    Off,
-    Offline,
-    Paused,
-    Stop,
+from isar.services.utilities.mqtt_utilities import publish_isar_status
+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.going_to_lockdown import GoingToLockdown
+from isar.state_machine.states.going_to_recharging import GoingToRecharging
+from isar.state_machine.states.home import Home
+from isar.state_machine.states.intervention_needed import InterventionNeeded
+from isar.state_machine.states.lockdown import Lockdown
+from isar.state_machine.states.maintenance import Maintenance
+from isar.state_machine.states.monitor import Monitor
+from isar.state_machine.states.offline import Offline
+from isar.state_machine.states.paused import Paused
+from isar.state_machine.states.pausing import Pausing
+from isar.state_machine.states.pausing_return_home import PausingReturnHome
+from isar.state_machine.states.recharging import Recharging
+from isar.state_machine.states.resuming import Resuming
+from isar.state_machine.states.resuming_return_home import ResumingReturnHome
+from isar.state_machine.states.return_home_paused import ReturnHomePaused
+from isar.state_machine.states.returning_home import ReturningHome
+from isar.state_machine.states.stopping import Stopping
+from isar.state_machine.states.stopping_due_to_maintenance import (
+    StoppingDueToMaintenance,
 )
+from isar.state_machine.states.stopping_go_to_lockdown import StoppingGoToLockdown
+from isar.state_machine.states.stopping_go_to_recharge import StoppingGoToRecharge
+from isar.state_machine.states.stopping_return_home import StoppingReturnHome
+from isar.state_machine.states.unknown_status import UnknownStatus
 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 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.mission.mission import Mission
-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.models.mission.task import ReturnToHome
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
+from robot_interface.telemetry.payloads import (
+    InterventionNeededPayload,
+    MissionAbortedPayload,
+)
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
 
 
 class StateMachine(object):
     """Handles state transitions for supervisory robot control."""
 
-    @inject
     def __init__(
         self,
-        queues: Queues,
+        events: Events,
+        shared_state: SharedState,
         robot: RobotInterface,
         mqtt_publisher: MqttClientInterface,
-        task_selector: TaskSelectorInterface,
-        sleep_time: float = settings.FSM_SLEEP_TIME,
-        stepwise_mission: bool = settings.RUN_MISSION_STEPWISE,
-        stop_robot_attempts_limit: int = settings.STOP_ROBOT_ATTEMPTS_LIMIT,
-        transitions_log_length: int = settings.STATE_TRANSITIONS_LOG_LENGTH,
     ):
         """Initializes the state machine.
 
         Parameters
         ----------
-        queues : Queues
-            Queues used for API communication.
+        events : Events
+            Events used for API and robot service communication.
         robot : RobotInterface
             Instance of robot interface.
         mqtt_publisher : MqttClientInterface
             Instance of MQTT client interface which has a publish function
-        sleep_time : float
-            Time to sleep in between state machine iterations.
-        stop_robot_attempts_limit : int
-            Maximum attempts to stop the robot when stop command is received
-        transitions_log_length : int
-            Length of state transition log list.
 
         """
         self.logger = logging.getLogger("state_machine")
 
-        self.queues: Queues = queues
+        self.events: Events = events
+        self.shared_state: SharedState = shared_state
         self.robot: RobotInterface = robot
         self.mqtt_publisher: Optional[MqttClientInterface] = mqtt_publisher
-        self.task_selector: TaskSelectorInterface = task_selector
-        self.stepwise_mission: bool = stepwise_mission
+
+        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)
-        self.initialize_state: State = Initialize(self)
+        # States running mission
         self.monitor_state: State = Monitor(self)
-        self.initiate_state: State = Initiate(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)
+        self.pausing_state: State = Pausing(self)
+        self.return_home_paused_state: State = ReturnHomePaused(self)
+        self.stopping_return_home_state: State = StoppingReturnHome(self)
+        self.pausing_return_home_state: State = PausingReturnHome(self)
+        self.resuming_state: State = Resuming(self)
+        self.resuming_return_home_state: State = ResumingReturnHome(self)
+        self.stopping_go_to_lockdown_state: State = StoppingGoToLockdown(self)
+        self.stopping_go_to_recharge_state: State = StoppingGoToRecharge(self)
+        self.going_to_lockdown_state: State = GoingToLockdown(self)
+        self.going_to_recharging_state: State = GoingToRecharging(self)
+        self.stopping_due_to_maintenance_state: State = StoppingDueToMaintenance(self)
+
+        # States Waiting for mission
+        self.await_next_mission_state: State = AwaitNextMission(self)
+        self.home_state: State = Home(self)
+        self.intervention_needed_state: State = InterventionNeeded(self)
+
+        # Status states
         self.offline_state: State = Offline(self)
+        self.blocked_protective_stopping_state: State = BlockedProtectiveStop(self)
+        self.recharging_state: State = Recharging(self)
+        self.lockdown_state: State = Lockdown(self)
+        self.maintenance_state: State = Maintenance(self)
+
+        # Error and special status states
+        self.unknown_status_state: State = UnknownStatus(self)
 
         self.states: List[State] = [
-            self.off_state,
-            self.idle_state,
-            self.initialize_state,
-            self.initiate_state,
             self.monitor_state,
-            self.stop_state,
+            self.returning_home_state,
+            self.stopping_state,
+            self.stopping_return_home_state,
+            self.pausing_return_home_state,
             self.paused_state,
+            self.pausing_state,
+            self.resuming_state,
+            self.return_home_paused_state,
+            self.await_next_mission_state,
+            self.home_state,
             self.offline_state,
+            self.blocked_protective_stopping_state,
+            self.unknown_status_state,
+            self.intervention_needed_state,
+            self.recharging_state,
+            self.stopping_go_to_lockdown_state,
+            self.resuming_return_home_state,
+            self.going_to_lockdown_state,
+            self.lockdown_state,
+            self.going_to_recharging_state,
+            self.stopping_go_to_recharge_state,
+            self.stopping_due_to_maintenance_state,
+            self.maintenance_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,
-                    "before": self._off,
-                },
-                {
-                    "trigger": "initiated",
-                    "source": self.initiate_state,
-                    "dest": self.monitor_state,
-                    "before": self._initiated,
-                },
-                {
-                    "trigger": "pause",
-                    "source": [self.initiate_state, self.monitor_state],
-                    "dest": self.stop_state,
-                    "before": self._pause,
-                },
-                {
-                    "trigger": "stop",
-                    "source": [self.initiate_state, self.monitor_state],
-                    "dest": self.stop_state,
-                    "before": self._stop,
-                },
-                {
-                    "trigger": "mission_finished",
-                    "source": [
-                        self.initiate_state,
-                    ],
-                    "dest": self.idle_state,
-                    "before": self._mission_finished,
-                },
-                {
-                    "trigger": "mission_started",
-                    "source": self.idle_state,
-                    "dest": self.initialize_state,
-                    "before": self._mission_started,
-                },
-                {
-                    "trigger": "initialization_successful",
-                    "source": self.initialize_state,
-                    "dest": self.initiate_state,
-                    "before": self._initialization_successful,
-                },
-                {
-                    "trigger": "initialization_failed",
-                    "source": self.initialize_state,
-                    "dest": self.idle_state,
-                    "before": self._initialization_failed,
-                },
-                {
-                    "trigger": "resume",
-                    "source": self.paused_state,
-                    "dest": self.initiate_state,
-                    "before": self._resume,
-                },
-                {
-                    "trigger": "step_finished",
-                    "source": self.monitor_state,
-                    "dest": self.initiate_state,
-                    "before": self._step_finished,
-                },
-                {
-                    "trigger": "full_mission_finished",
-                    "source": self.monitor_state,
-                    "dest": self.initiate_state,
-                    "before": self._full_mission_finished,
-                },
-                {
-                    "trigger": "mission_paused",
-                    "source": self.stop_state,
-                    "dest": self.paused_state,
-                    "before": self._mission_paused,
-                },
-                {
-                    "trigger": "initiate_infeasible",
-                    "source": self.initiate_state,
-                    "dest": self.initiate_state,
-                    "before": self._initiate_infeasible,
-                },
-                {
-                    "trigger": "initiate_failed",
-                    "source": self.initiate_state,
-                    "dest": self.idle_state,
-                    "before": self._initiate_failed,
-                },
-                {
-                    "trigger": "mission_stopped",
-                    "source": [self.stop_state, self.paused_state],
-                    "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,
-                },
-            ]
-        )
-
-        self.stop_robot_attempts_limit: int = stop_robot_attempts_limit
-        self.sleep_time: float = sleep_time
-
-        self.stopped: bool = False
-        self.current_mission: Optional[Mission] = None
-        self.current_task: Optional[Task] = None
-        self.current_step: Optional[Step] = None
-        self.initial_pose: Optional[Pose] = None
-
-        self.current_state: State = States(self.state)  # type: ignore
-
-        self.predefined_mission_id: Optional[int] = None
-
-        self.transitions_log_length: int = transitions_log_length
-        self.transitions_list: Deque[States] = deque([], self.transitions_log_length)
+        if settings.PERSISTENT_STORAGE_CONNECTION_STRING == "":
+            initial_state = "unknown_status"
+            self.logger.warning(
+                "PERSISTENT_STORAGE_CONNECTION_STRING is not set. Restarting ISAR will forget the state, including maintenance mode. "
+            )
+        else:
+            is_maintenance_mode = read_or_create_persistent_maintenance_mode()
+            self.logger.info(
+                f"Connected to robot status database and the maintenance mode was: {is_maintenance_mode}. "
+            )
+            if is_maintenance_mode:
+                initial_state = "maintenance"
+            else:
+                initial_state = "unknown_status"
 
-    #################################################################################
-    # Transition Callbacks
-    def _initialization_successful(self) -> None:
-        return
-
-    def _initialization_failed(self) -> None:
-        self.queues.start_mission.output.put(False)
-        self._finalize()
-
-    def _initiated(self) -> None:
-        if self.stepwise_mission:
-            self.current_step.status = StepStatus.InProgress
-        self.current_mission.status = MissionStatus.InProgress
-        self.publish_step_status(step=self.current_step)
-        self.logger.info(
-            f"Successfully initiated "
-            f"{type(self.current_step).__name__} "
-            f"step: {str(self.current_step.id)[:8]}"
+        self.machine = Machine(
+            self, states=self.states, initial=initial_state, queued=True
         )
 
-    def _pause(self) -> None:
-        return
-
-    def _off(self) -> None:
-        return
-
-    def _offline(self) -> None:
-        return
-
-    def _online(self) -> None:
-        return
+        self.transitions: List[dict] = []
 
-    def _resume(self) -> None:
-        self.logger.info(f"Resuming mission: {self.current_mission.id}")
-        self.current_mission.status = MissionStatus.InProgress
-        self.current_mission.error_message = None
-        self.current_task.status = TaskStatus.InProgress
+        self.transitions.extend(get_mission_transitions(self))
+        self.transitions.extend(get_return_home_transitions(self))
+        self.transitions.extend(get_robot_status_transitions(self))
 
-        self.publish_mission_status()
-        self.publish_task_status(task=self.current_task)
+        self.machine.add_transitions(self.transitions)
 
-        resume_mission_response: ControlMissionResponse = (
-            self._make_control_mission_response()
-        )
-        self.queues.resume_mission.output.put(resume_mission_response)
-
-        self.current_task.reset_task()
-        self.update_current_step()
-
-    def _mission_finished(self) -> None:
-        fail_statuses: List[TaskStatus] = [
-            TaskStatus.Cancelled,
-            TaskStatus.Failed,
-        ]
-        partially_fail_statuses = fail_statuses + [TaskStatus.PartiallySuccessful]
-
-        if len(self.current_mission.tasks) == 0:
-            self.current_mission.status = MissionStatus.Successful
-        elif all(task.status in fail_statuses for task in self.current_mission.tasks):
-            self.current_mission.error_message = ErrorMessage(
-                error_reason=None,
-                error_description="The mission failed because all tasks in the mission "
-                "failed",
-            )
-            self.current_mission.status = MissionStatus.Failed
-        elif any(
-            task.status in partially_fail_statuses
-            for task in self.current_mission.tasks
-        ):
-            self.current_mission.status = MissionStatus.PartiallySuccessful
-        else:
-            self.current_mission.status = MissionStatus.Successful
-        self._finalize()
-
-    def _mission_started(self) -> None:
-        self.queues.start_mission.output.put(True)
-        self.logger.info(
-            f"Initialization successful. Starting new mission: "
-            f"{self.current_mission.id}"
-        )
-        self.log_step_overview(mission=self.current_mission)
+        self.current_state: State = States(self.state)  # type: ignore
 
-        self.current_mission.status = MissionStatus.InProgress
-        self.publish_mission_status()
-        self.current_task = self.task_selector.next_task()
-        if self.current_task == None:
-            self._mission_finished()
-        else:
-            self.current_task.status = TaskStatus.InProgress
-            self.publish_task_status(task=self.current_task)
-            self.update_current_step()
-
-    def _step_finished(self) -> None:
-        self.publish_step_status(step=self.current_step)
-        self.update_current_task()
-        self.update_current_step()
-
-    def _full_mission_finished(self) -> None:
-        self.current_task = None
-
-    def _mission_paused(self) -> None:
-        self.logger.info(f"Pausing mission: {self.current_mission.id}")
-        self.current_mission.status = MissionStatus.Paused
-        self.current_task.status = TaskStatus.Paused
-        self.current_step.status = StepStatus.NotStarted
-
-        paused_mission_response: ControlMissionResponse = (
-            self._make_control_mission_response()
-        )
-        self.queues.pause_mission.output.put(paused_mission_response)
-
-        self.publish_mission_status()
-        self.publish_task_status(task=self.current_task)
-        self.publish_step_status(step=self.current_step)
-
-    def _stop(self) -> None:
-        self.stopped = True
-
-    def _initiate_failed(self) -> None:
-        self.current_step.status = StepStatus.Failed
-        self.current_task.update_task_status()
-        self.current_mission.status = MissionStatus.Failed
-        self.publish_step_status(step=self.current_step)
-        self.publish_task_status(task=self.current_task)
-        self._finalize()
-
-    def _initiate_infeasible(self) -> None:
-        if self.stepwise_mission:
-            self.current_step.status = StepStatus.Failed
-            self.publish_step_status(step=self.current_step)
-            self.update_current_task()
-            self.update_current_step()
-
-    def _mission_stopped(self) -> None:
-        self.current_mission.status = MissionStatus.Cancelled
-
-        for task in self.current_mission.tasks:
-            for step in task.steps:
-                if step.status in [StepStatus.NotStarted, StepStatus.InProgress]:
-                    step.status = StepStatus.Cancelled
-            if task.status in [
-                TaskStatus.NotStarted,
-                TaskStatus.InProgress,
-                TaskStatus.Paused,
-            ]:
-                task.status = TaskStatus.Cancelled
-
-        stopped_mission_response: ControlMissionResponse = (
-            self._make_control_mission_response()
+        self.transitions_list: Deque[States] = deque(
+            [], settings.STATE_TRANSITIONS_LOG_LENGTH
         )
-        self.queues.stop_mission.output.put(stopped_mission_response)
-
-        self.publish_task_status(task=self.current_task)
-        self.publish_step_status(step=self.current_step)
-        self._finalize()
 
     #################################################################################
 
-    def _finalize(self) -> None:
-        self.publish_mission_status()
-        self.log_step_overview(mission=self.current_mission)
+    def print_transitions(self) -> None:
         state_transitions: str = ", ".join(
             [
                 f"\n  {transition}" if (i + 1) % 10 == 0 else f"{transition}"
                 for i, transition in enumerate(list(self.transitions_list))
             ]
         )
-        self.logger.info(f"State transitions:\n  {state_transitions}")
-        self.reset_state_machine()
+        self.logger.info("State transitions:\n  %s", state_transitions)
+        self.transitions = []
 
     def begin(self):
-        """Starts the state machine.
+        """Starts the state machine. Transitions into unknown status state."""
+        self.initial_transition()  # type: ignore
 
-        Transitions into idle state.
+    def terminate(self):
+        self.logger.info("Stopping state machine")
+        self.signal_state_machine_to_stop.set()
 
-        """
-        self.to_idle()
-
-    def update_current_task(self):
-        if self.current_task.is_finished():
-            self.current_task.update_task_status()
-            self.publish_task_status(task=self.current_task)
-            try:
-                self.current_task = self.task_selector.next_task()
-                self.current_task.status = TaskStatus.InProgress
-                self.publish_task_status(task=self.current_task)
-            except TaskSelectorStop:
-                # Indicates that all tasks are finished
-                self.current_task = None
-
-    def update_current_step(self):
-        if self.current_task != None:
-            self.current_step = self.current_task.next_step()
-
-    def update_remaining_steps(self):
-        if self.current_task:
-            for step in self.current_task.steps:
-                if (
-                    step.status == StepStatus.InProgress
-                    or step.status == StepStatus.NotStarted
-                ):
-                    step.status = self.current_task.status
-                    self.publish_step_status(step=step)
+    def battery_level_is_above_mission_start_threshold(self):
+        if not self.shared_state.robot_battery_level.check():
+            self.logger.warning("Battery level is None")
+            return False
+        return (
+            not self.shared_state.robot_battery_level.check()
+            < settings.ROBOT_MISSION_BATTERY_START_THRESHOLD
+        )
 
     def update_state(self):
         """Updates the current state of the state machine."""
-        self.current_state = States(self.state)
-        self.send_state_status()
-        self._log_state_transition(self.current_state)
-        self.logger.info(f"State: {self.current_state}")
+        self.current_state = States(self.state)  # type: ignore
+        self.shared_state.state.update(self.current_state)
+        self.transitions_list.append(self.current_state)
+        self.logger.info("State: %s", self.current_state)
         self.publish_status()
 
-    def reset_state_machine(self) -> None:
-        self.logger.info("Resetting state machine")
-        self.stopped = False
-        self.current_step = None
-        self.current_task = None
-        self.current_mission = None
-        self.initial_pose = None
-
-    def start_mission(self, mission: Mission, initial_pose: Pose):
+    def start_mission(self, mission: Mission):
         """Starts a scheduled mission."""
-        self.current_mission = mission
-        self.initial_pose = initial_pose
-
-        self.task_selector.initialize(tasks=self.current_mission.tasks)
-
-    def get_initialize_params(self):
-        return InitializeParams(initial_pose=self.initial_pose)
+        self.events.state_machine_events.start_mission.trigger_event(mission)
 
-    def should_start_mission(self) -> Optional[StartMissionMessage]:
-        try:
-            return self.queues.start_mission.input.get(block=False)
-        except queue.Empty:
-            return None
-
-    def should_stop_mission(self) -> bool:
-        try:
-            return self.queues.stop_mission.input.get(block=False)
-        except queue.Empty:
-            return False
-
-    def should_pause_mission(self) -> bool:
-        try:
-            return self.queues.pause_mission.input.get(block=False)
-        except queue.Empty:
-            return False
-
-    def should_resume_mission(self) -> bool:
-        try:
-            return self.queues.resume_mission.input.get(block=False)
-        except queue.Empty:
-            return False
-
-    def send_state_status(self):
-        self.queues.state.update(self.current_state)
+    def start_return_home_mission(self):
+        """Starts a return to home mission."""
+        mission = Mission(
+            tasks=[ReturnToHome()],
+            name="Return Home",
+        )
+        self.events.state_machine_events.start_mission.trigger_event(mission)
 
-    def publish_mission_status(self) -> None:
+    def publish_mission_aborted(self, reason: str, can_be_continued: bool) -> None:
         if not self.mqtt_publisher:
             return
 
-        error_message: Optional[ErrorMessage] = None
-        if self.current_mission:
-            if self.current_mission.error_message:
-                error_message = self.current_mission.error_message
-        payload: str = json.dumps(
-            {
-                "isar_id": settings.ISAR_ID,
-                "robot_name": settings.ROBOT_NAME,
-                "mission_id": self.current_mission.id if self.current_mission else None,
-                "status": self.current_mission.status if self.current_mission else None,
-                "error_reason": error_message.error_reason if error_message else None,
-                "error_description": (
-                    error_message.error_description if error_message else None
-                ),
-                "timestamp": datetime.now(UTC),
-            },
-            cls=EnhancedJSONEncoder,
-        )
-
-        self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_MISSION,
-            payload=payload,
-            qos=1,
-            retain=True,
-        )
-
-    def publish_task_status(self, task: Task) -> None:
-        """Publishes the task status to the MQTT Broker"""
-        if not self.mqtt_publisher:
+        if self.shared_state.mission_id.check() is None:
+            self.logger.warning(
+                "Could not publish mission aborted message. No ongoing mission."
+            )
             return
 
-        error_message: Optional[ErrorMessage] = None
-        if task:
-            if task.error_message:
-                error_message = task.error_message
-
-        payload: str = json.dumps(
-            {
-                "isar_id": settings.ISAR_ID,
-                "robot_name": settings.ROBOT_NAME,
-                "mission_id": self.current_mission.id if self.current_mission else None,
-                "task_id": task.id if task else None,
-                "status": task.status if task else None,
-                "error_reason": error_message.error_reason if error_message else None,
-                "error_description": (
-                    error_message.error_description if error_message else None
-                ),
-                "timestamp": datetime.now(UTC),
-            },
-            cls=EnhancedJSONEncoder,
+        payload: MissionAbortedPayload = MissionAbortedPayload(
+            isar_id=settings.ISAR_ID,
+            robot_name=settings.ROBOT_NAME,
+            mission_id=self.shared_state.mission_id.check(),
+            reason=reason,
+            can_be_continued=can_be_continued,
+            timestamp=datetime.now(timezone.utc),
         )
 
         self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_TASK,
-            payload=payload,
+            topic=settings.TOPIC_ISAR_MISSION_ABORTED,
+            payload=json.dumps(payload, cls=EnhancedJSONEncoder),
             qos=1,
             retain=True,
         )
 
-    def publish_step_status(self, step: Step) -> None:
-        """Publishes the step status to the MQTT Broker"""
+    def publish_intervention_needed(self, error_message: str) -> None:
+        """Publishes the intervention needed message to the MQTT Broker"""
         if not self.mqtt_publisher:
             return
 
-        error_message: Optional[ErrorMessage] = None
-        if step:
-            if step.error_message:
-                error_message = step.error_message
-
-        payload: str = json.dumps(
-            {
-                "isar_id": settings.ISAR_ID,
-                "robot_name": settings.ROBOT_NAME,
-                "mission_id": self.current_mission.id if self.current_mission else None,
-                "task_id": self.current_task.id if self.current_task else None,
-                "step_id": step.id if step else None,
-                "step_type": step.__class__.__name__ if step else None,
-                "status": step.status if step else None,
-                "error_reason": error_message.error_reason if error_message else None,
-                "error_description": (
-                    error_message.error_description if error_message else None
-                ),
-                "timestamp": datetime.now(UTC),
-            },
-            cls=EnhancedJSONEncoder,
+        payload: InterventionNeededPayload = InterventionNeededPayload(
+            isar_id=settings.ISAR_ID,
+            robot_name=settings.ROBOT_NAME,
+            reason=error_message,
+            timestamp=datetime.now(timezone.utc),
         )
 
         self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_STEP,
-            payload=payload,
+            topic=settings.TOPIC_ISAR_INTERVENTION_NEEDED,
+            payload=json.dumps(payload, cls=EnhancedJSONEncoder),
             qos=1,
             retain=True,
         )
@@ -591,60 +279,53 @@ class StateMachine(object):
     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,
-                "status": self._current_status(),
-                "timestamp": datetime.now(UTC),
-            },
-            cls=EnhancedJSONEncoder,
-        )
-
-        self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_STATUS,
-            payload=payload,
-            qos=1,
-            retain=True,
-        )
 
-    def _current_status(self) -> RobotStatus:
-        if self.current_state == States.Idle:
-            return RobotStatus.Available
+        publish_isar_status(self.mqtt_publisher, self._current_status())
+
+    def _current_status(self) -> IsarStatus:
+        if self.current_state == States.AwaitNextMission:
+            return IsarStatus.Available
+        elif self.current_state == States.ReturnHomePaused:
+            return IsarStatus.ReturnHomePaused
+        elif self.current_state == States.Paused:
+            return IsarStatus.Paused
+        elif self.current_state == States.Home:
+            return IsarStatus.Home
+        elif self.current_state == States.ReturningHome:
+            return IsarStatus.ReturningHome
         elif self.current_state == States.Offline:
-            return RobotStatus.Offline
+            return IsarStatus.Offline
+        elif self.current_state == States.BlockedProtectiveStop:
+            return IsarStatus.BlockedProtectiveStop
+        elif self.current_state == States.InterventionNeeded:
+            return IsarStatus.InterventionNeeded
+        elif self.current_state == States.Recharging:
+            return IsarStatus.Recharging
+        elif self.current_state == States.Lockdown:
+            return IsarStatus.Lockdown
+        elif self.current_state == States.GoingToLockdown:
+            return IsarStatus.GoingToLockdown
+        elif self.current_state == States.GoingToRecharging:
+            return IsarStatus.GoingToRecharging
+        elif self.current_state == States.Maintenance:
+            return IsarStatus.Maintenance
         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)
-
-    def log_step_overview(self, mission: Mission):
-        """Log an overview of the steps in a mission"""
-        log_statements: List[str] = []
-        for task in mission.tasks:
-            log_statements.append(
-                f"{type(task).__name__:<20} {str(task.id)[:8]:<32} -- {task.status}"
-            )
-            for j, step in enumerate(task.steps):
-                log_statements.append(
-                    f"{j:>3} {type(step).__name__:<20} {str(step.id)[:8]:<32} -- {step.status}"  # noqa: E501
-                )
-
-        log_statement: str = "\n".join(log_statements)
-
-        self.logger.info(f"Mission overview:\n{log_statement}")
-
-    def _make_control_mission_response(self) -> ControlMissionResponse:
-        return ControlMissionResponse(
-            mission_id=self.current_mission.id,
-            mission_status=self.current_mission.status,
-            task_id=self.current_task.id,
-            task_status=self.current_task.status,
-            step_id=self.current_step.id,
-            step_status=self.current_step.status,
+            return IsarStatus.Busy
+
+
+def read_or_create_persistent_maintenance_mode():
+    try:
+        is_maintenance_mode = read_persistent_robot_state_is_maintenance_mode(
+            settings.PERSISTENT_STORAGE_CONNECTION_STRING, settings.ISAR_ID
+        )
+    except NoSuchRobotException:
+        create_persistent_robot_state(
+            settings.PERSISTENT_STORAGE_CONNECTION_STRING, settings.ISAR_ID
+        )
+        is_maintenance_mode = read_persistent_robot_state_is_maintenance_mode(
+            settings.PERSISTENT_STORAGE_CONNECTION_STRING, settings.ISAR_ID
         )
+    return is_maintenance_mode
 
 
 def main(state_machine: StateMachine):