isar 1.32.3__tar.gz → 1.33.0__tar.gz

{isar-1.32.3 → isar-1.33.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: isar
-Version: 1.32.3
+Version: 1.33.0
 Summary: Integration and Supervisory control of Autonomous Robots
 Author-email: Equinor ASA <fg_robots_dev@equinor.com>
 License: Eclipse Public License version 2.0

{isar-1.32.3 → isar-1.33.0}/docs/update_state_diagram.py

@@ -13,9 +13,59 @@ from isar.state_machine.transitions.return_home import get_return_home_transitio
 from isar.state_machine.transitions.robot_status import get_robot_status_transitions
 
 
+def extract_function_name_from_callable(func):
+    try:
+        closure = getattr(func, "__closure__", None)
+        if closure:
+            for cell in closure:
+                obj = cell.cell_contents
+                if callable(obj):
+                    return getattr(obj, "__name__", str(obj))
+
+        return getattr(func, "__name__", str(func))
+    except Exception:
+        return "unknown"
+
+
+def embed_conditions_in_trigger(transitions: List[dict]) -> List[dict]:
+    updated = []
+    for t in transitions:
+        trigger = t.get("trigger", "")
+        conditions = t.get("conditions", [])
+
+        if not isinstance(conditions, list):
+            conditions = [conditions]
+
+        condition_names = []
+
+        for cond in conditions:
+            name = extract_function_name_from_callable(cond)
+            condition_names.append(name)
+
+        befores = t.get("before", [])
+        if not isinstance(befores, list):
+            befores = [befores]
+        before_names = [extract_function_name_from_callable(b) for b in befores]
+
+        label_parts = [trigger]
+        if condition_names:
+            label_parts.append(f"[{'; '.join(condition_names)}]")
+        if before_names:
+            label_parts.append(f"/ {'; '.join(before_names)}")
+
+        new_trigger = " ".join(label_parts)
+
+        t_copy = t.copy()
+        t_copy["trigger"] = new_trigger
+        updated.append(t_copy)
+
+    return updated
+
+
 def draw_diagram(states: List[State], transitions: List[dict], name: str):
+    transitions_with_conditions = embed_conditions_in_trigger(transitions)
     machine = GraphMachine(states=states, initial="unknown_status", queued=True)
-    machine.add_transitions(transitions)
+    machine.add_transitions(transitions_with_conditions)
     gp = machine.get_combined_graph()
 
     state_machine_diagram_file = (
@@ -25,12 +75,12 @@ def draw_diagram(states: List[State], transitions: List[dict], name: str):
     if os.path.isfile(state_machine_diagram_file):
         os.remove(state_machine_diagram_file)
 
-    gp.draw(state_machine_diagram_file, prog="dot")
+    gp.draw(str(state_machine_diagram_file), prog="dot", format="png")
 
 
 if __name__ == "__main__":
     injector: Injector = get_injector()
-    state_machine: StateMachine = injector.get(StateMachine)
+    state_machine: StateMachine = injector.state_machine()
 
     mission_extended_transitions: List[dict] = []
     for transition in get_mission_transitions(state_machine):

{isar-1.32.3 → isar-1.33.0}/src/isar/apis/models/models.py

@@ -26,6 +26,12 @@ class ControlMissionResponse(BaseModel):
     task_status: Optional[str]
 
 
+class MissionStartResponse(BaseModel):
+    mission_id: Optional[str] = None
+    mission_started: bool
+    mission_not_started_reason: Optional[str] = None
+
+
 class RobotInfoResponse(BaseModel):
     robot_package: str
     isar_id: str

{isar-1.32.3 → isar-1.33.0}/src/isar/config/open_telemetry.py

@@ -1,4 +1,5 @@
 import logging
+from urllib.parse import urljoin
 
 from azure.monitor.opentelemetry.exporter import (
     AzureMonitorLogExporter,
@@ -7,6 +8,12 @@ from azure.monitor.opentelemetry.exporter import (
 from fastapi import FastAPI
 from opentelemetry import trace
 from opentelemetry._logs import set_logger_provider
+from opentelemetry.exporter.otlp.proto.http._log_exporter import (
+    OTLPLogExporter as OTLPHttpLogExporter,
+)
+from opentelemetry.exporter.otlp.proto.http.trace_exporter import (
+    OTLPSpanExporter as OTLPHttpSpanExporter,
+)
 from opentelemetry.instrumentation.fastapi import FastAPIInstrumentor
 from opentelemetry.sdk._logs import LoggerProvider, LoggingHandler
 from opentelemetry.sdk._logs.export import BatchLogRecordProcessor
@@ -17,22 +24,45 @@ from opentelemetry.sdk.trace.export import BatchSpanProcessor
 from isar.config.log import load_log_config
 from isar.config.settings import settings
 
+logging.getLogger("opentelemetry.sdk").setLevel(logging.CRITICAL)
+
 
 def setup_open_telemetry(app: FastAPI) -> None:
-    if not settings.LOG_HANDLER_APPLICATION_INSIGHTS_ENABLED:
-        return
-    trace_exporter, log_exporter = get_azure_monitor_exporters()
 
-    service_name = settings.OPEN_TELEMETRY_SERVICE_NAME
+    service_name = settings.ROBOT_NAME
     resource = Resource.create({SERVICE_NAME: service_name})
 
     tracer_provider = TracerProvider(resource=resource)
-    tracer_provider.add_span_processor(BatchSpanProcessor(trace_exporter))
-    trace.set_tracer_provider(tracer_provider)
-
     log_provider = LoggerProvider(resource=resource)
+
+    if settings.LOG_HANDLER_APPLICATION_INSIGHTS_ENABLED:
+        print("[OTEL] Azure Monitor exporters enabled")
+        azure_monitor_trace_exporter, azure_monitor_log_exporter = (
+            get_azure_monitor_exporters()
+        )
+
+        tracer_provider.add_span_processor(
+            BatchSpanProcessor(azure_monitor_trace_exporter)
+        )
+
+        log_provider.add_log_record_processor(
+            BatchLogRecordProcessor(azure_monitor_log_exporter)
+        )
+
+    otlp_exporter_endpoint = settings.OPEN_TELEMETRY_OTLP_EXPORTER_ENDPOINT
+    if otlp_exporter_endpoint:
+        print(f"[OTEL] OTLP exporters enabled, endpoint={otlp_exporter_endpoint}")
+        otlp_trace_exporter, otlp_log_exporter = get_otlp_exporters(
+            otlp_exporter_endpoint
+        )
+        tracer_provider.add_span_processor(BatchSpanProcessor(otlp_trace_exporter))
+
+        log_provider.add_log_record_processor(
+            BatchLogRecordProcessor(otlp_log_exporter)
+        )
+
     set_logger_provider(log_provider)
-    log_provider.add_log_record_processor(BatchLogRecordProcessor(log_exporter))
+    trace.set_tracer_provider(tracer_provider)
 
     handler = LoggingHandler(logger_provider=log_provider)
     attach_loggers_for_open_telemetry(handler)
@@ -51,12 +81,22 @@ def attach_loggers_for_open_telemetry(handler: LoggingHandler):
 def get_azure_monitor_exporters() -> (
     tuple[AzureMonitorTraceExporter, AzureMonitorLogExporter]
 ):
-    """
-    If connection string is defined in environment variables, then use it to create Azure Monitor Exporters.
-    Else use Azure Managed Identity to create Azure Monitor Exporters.
-    """
     connection_string = settings.APPLICATIONINSIGHTS_CONNECTION_STRING
     trace_exporter = AzureMonitorTraceExporter(connection_string=connection_string)
     log_exporter = AzureMonitorLogExporter(connection_string=connection_string)
 
     return trace_exporter, log_exporter
+
+
+def get_otlp_exporters(
+    endpoint: str,
+) -> tuple[OTLPHttpSpanExporter, OTLPHttpLogExporter]:
+    base = endpoint.rstrip("/") + "/"
+    trace_ep = urljoin(base, "v1/traces")
+    log_ep = urljoin(base, "v1/logs")
+
+    print("[OTEL] Using HTTP/Protobuf protocol for OpenTelemetry export")
+    print(f"[OTEL]  traces → {trace_ep}")
+    print(f"[OTEL]  logs   → {log_ep}")
+
+    return OTLPHttpSpanExporter(endpoint=trace_ep), OTLPHttpLogExporter(endpoint=log_ep)

{isar-1.32.3 → isar-1.33.0}/src/isar/config/settings.py

@@ -12,8 +12,9 @@ from robot_interface.telemetry.payloads import DocumentInfo
 
 
 class Settings(BaseSettings):
-    # Name of the OpenTelemetry service
-    OPEN_TELEMETRY_SERVICE_NAME: str = Field(default="isar")
+    # Endpoint open telemetry will export telemetry in the otlp protocol to
+    OPEN_TELEMETRY_OTLP_EXPORTER_ENDPOINT: str = Field(default="http://localhost:4318")
+
     # Connection string for Azure Application Insights
     # This is optional and it will use managed identity if not set
     APPLICATIONINSIGHTS_CONNECTION_STRING: str = Field(default="")
@@ -29,7 +30,7 @@ class Settings(BaseSettings):
     REQUEST_TIMEOUT: int = Field(default=30)
 
     # Timeout in seconds for checking whether there is a message on a queue
-    QUEUE_TIMEOUT: int = Field(default=10)
+    QUEUE_TIMEOUT: int = Field(default=3)
 
     # Sleep time for while loops in the finite state machine in seconds
     # The sleep is used to throttle the system on every iteration in the loop
@@ -80,6 +81,15 @@ class Settings(BaseSettings):
     ROBOT_API_STATUS_POLL_INTERVAL: float = Field(default=5)
     THREAD_CHECK_INTERVAL: float = Field(default=0.01)
 
+    # Determines the minimum battery level the robot must have to start a mission
+    # If it drops below this level it will recharge to the value set by
+    # ROBOT_BATTERY_RECHARGE_THRESHOLD before starting new missions
+    ROBOT_MISSION_BATTERY_START_THRESHOLD: float = Field(default=25.0)
+
+    # Determines the minimum battery threshold to consider the robot recharged
+    # and ready for more missions, after having run low on charge
+    ROBOT_BATTERY_RECHARGE_THRESHOLD: float = Field(default=80.0)
+
     # FastAPI host
     API_HOST_VIEWED_EXTERNALLY: str = Field(default="0.0.0.0")
 
@@ -215,6 +225,9 @@ class Settings(BaseSettings):
     # List of MQTT Topics
     TOPIC_ISAR_STATUS: str = Field(default="status", validate_default=True)
     TOPIC_ISAR_MISSION: str = Field(default="mission", validate_default=True)
+    TOPIC_ISAR_MISSION_ABORTED: str = Field(
+        default="aborted_mission", validate_default=True
+    )
     TOPIC_ISAR_TASK: str = Field(default="task", validate_default=True)
     TOPIC_ISAR_INSPECTION_RESULT: str = Field(
         default="inspection_result", validate_default=True
@@ -282,6 +295,7 @@ class Settings(BaseSettings):
         "TOPIC_ISAR_INSPECTION_VALUE",
         "TOPIC_ISAR_STARTUP",
         "TOPIC_ISAR_INTERVENTION_NEEDED",
+        "TOPIC_ISAR_MISSION_ABORTED",
     )
     @classmethod
     def prefix_isar_topics(cls, v: Any, info: ValidationInfo):

{isar-1.32.3 → isar-1.33.0}/src/isar/eventhandlers/eventhandler.py

@@ -58,6 +58,28 @@ class EventHandlerBase(State):
     def stop(self) -> None:
         return
 
+    def get_event_handler_by_name(
+        self, event_handler_name: str
+    ) -> Optional[EventHandlerMapping]:
+        filtered_handlers = list(
+            filter(
+                lambda mapping: mapping.name == event_handler_name,
+                self.event_handler_mappings,
+            )
+        )
+        return filtered_handlers[0] if len(filtered_handlers) > 0 else None
+
+    def get_event_timer_by_name(
+        self, event_timer_name: str
+    ) -> Optional[TimeoutHandlerMapping]:
+        filtered_timers = list(
+            filter(
+                lambda mapping: mapping.name == event_timer_name,
+                self.timers,
+            )
+        )
+        return filtered_timers[0] if len(filtered_timers) > 0 else None
+
     def _run(self) -> None:
         should_exit_state: bool = False
         timers = deepcopy(self.timers)

{isar-1.32.3 → isar-1.33.0}/src/isar/models/events.py

@@ -4,7 +4,7 @@ from typing import Generic, Optional, TypeVar
 
 from transitions import State
 
-from isar.apis.models.models import ControlMissionResponse
+from isar.apis.models.models import ControlMissionResponse, MissionStartResponse
 from isar.config.settings import settings
 from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
 from robot_interface.models.mission.mission import Mission
@@ -20,8 +20,13 @@ class Event(Queue[T]):
     def __init__(self) -> None:
         super().__init__(maxsize=1)
 
-    def trigger_event(self, data: T) -> None:
-        self.put(data)
+    def trigger_event(self, data: T, timeout: int = None) -> None:
+        try:
+            self.put(data, timeout=timeout)
+        except Exception:
+            if timeout is not None:
+                raise EventTimeoutError
+            return None
 
     def consume_event(self, timeout: int = None) -> Optional[T]:
         try:
@@ -81,7 +86,7 @@ class APIEvent(Generic[T1, T2]):
 
 class APIRequests:
     def __init__(self) -> None:
-        self.start_mission: APIEvent[Mission, bool] = APIEvent()
+        self.start_mission: APIEvent[Mission, MissionStartResponse] = APIEvent()
         self.stop_mission: APIEvent[str, ControlMissionResponse] = APIEvent()
         self.pause_mission: APIEvent[bool, ControlMissionResponse] = APIEvent()
         self.resume_mission: APIEvent[bool, ControlMissionResponse] = APIEvent()
@@ -113,7 +118,12 @@ class SharedState:
         self.state: Event[State] = Event()
         self.robot_status: Event[RobotStatus] = Event()
         self.state_machine_current_task: Event[TASKS] = Event()
+        self.robot_battery_level: Event[float] = Event()
 
 
 class EventTimeoutError(Exception):
     pass
+
+
+class EventConflictError(Exception):
+    pass

{isar-1.32.3 → isar-1.33.0}/src/isar/robot/robot_status.py

@@ -48,7 +48,10 @@ class RobotStatusThread(Thread):
                 self.last_robot_status_poll_time = time.time()
 
                 robot_status = self.robot.robot_status()
+                robot_battery_level = self.robot.get_battery_level()
+
                 self.shared_state.robot_status.update(robot_status)
+                self.shared_state.robot_battery_level.update(robot_battery_level)
             except RobotException as e:
                 self.logger.error(f"Failed to retrieve robot status: {e}")
                 continue

{isar-1.32.3 → isar-1.33.0}/src/isar/services/utilities/scheduling_utilities.py

@@ -16,6 +16,7 @@ from isar.mission_planner.mission_planner_interface import (
 from isar.models.events import (
     APIEvent,
     APIRequests,
+    EventConflictError,
     Events,
     EventTimeoutError,
     SharedState,
@@ -176,16 +177,28 @@ class SchedulingUtilities:
             If there is a timeout while communicating with the state machine
         """
         try:
-            self._send_command(
+            mission_start_response = self._send_command(
                 deepcopy(mission),
                 self.api_events.start_mission,
             )
+            if not mission_start_response.mission_started:
+                self.logger.warning(
+                    f"Mission failed to start - {mission_start_response.mission_not_started_reason}"
+                )
+                raise HTTPException(
+                    status_code=HTTPStatus.CONFLICT,
+                    detail=mission_start_response.mission_not_started_reason,
+                )
+        except EventConflictError:
+            error_message = "Previous mission request is still being processed"
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         except EventTimeoutError:
-            error_message = "Internal Server Error - Failed to start mission in ISAR"
-            self.logger.error(error_message)
-            raise HTTPException(
-                status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
+            error_message = (
+                "State machine has entered a state which cannot start a mission"
             )
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         self.logger.info("OK - Mission started in ISAR")
 
     def return_home(
@@ -203,14 +216,14 @@ class SchedulingUtilities:
                 True,
                 self.api_events.return_home,
             )
+        except EventConflictError:
+            error_message = "Previous return home request is still being processed"
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         except EventTimeoutError:
-            error_message = (
-                "Internal Server Error - Failed to start return home mission in ISAR"
-            )
-            self.logger.error(error_message)
-            raise HTTPException(
-                status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
-            )
+            error_message = "State machine has entered a state which cannot start a return home mission"
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         self.logger.info("OK - Return home mission started in ISAR")
 
     def pause_mission(self) -> ControlMissionResponse:
@@ -225,12 +238,16 @@ class SchedulingUtilities:
             response = self._send_command(True, self.api_events.pause_mission)
             self.logger.info("OK - Mission successfully paused")
             return response
+        except EventConflictError:
+            error_message = "Previous pause mission request is still being processed"
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         except EventTimeoutError:
-            error_message = "Internal Server Error - Failed to pause mission"
-            self.logger.error(error_message)
-            raise HTTPException(
-                status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
+            error_message = (
+                "State machine has entered a state which cannot pause a mission"
             )
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
 
     def resume_mission(self) -> ControlMissionResponse:
         """Resume mission
@@ -244,6 +261,10 @@ class SchedulingUtilities:
             response = self._send_command(True, self.api_events.resume_mission)
             self.logger.info("OK - Mission successfully resumed")
             return response
+        except EventConflictError:
+            error_message = "Previous resume mission request is still being processed"
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         except EventTimeoutError:
             error_message = "Internal Server Error - Failed to resume mission"
             self.logger.error(error_message)
@@ -281,12 +302,16 @@ class SchedulingUtilities:
                 raise HTTPException(
                     status_code=HTTPStatus.CONFLICT, detail=error_message
                 )
+        except EventConflictError:
+            error_message = "Previous stop mission request is still being processed"
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         except EventTimeoutError:
-            error_message = "Internal Server Error - Failed to stop mission"
-            self.logger.error(error_message)
-            raise HTTPException(
-                status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
+            error_message = (
+                "State machine has entered a state which cannot stop a mission"
             )
+            self.logger.warning(error_message)
+            raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
         self.logger.info("OK - Mission successfully stopped")
         return stop_mission_response
 
@@ -311,8 +336,11 @@ class SchedulingUtilities:
             )
 
     def _send_command(self, input: T1, api_event: APIEvent[T1, T2]) -> T2:
-        api_event.request.trigger_event(input)
+        if api_event.request.has_event() or api_event.response.has_event():
+            raise EventConflictError("API event has already been sent")
+
         try:
+            api_event.request.trigger_event(input, timeout=1)
             return api_event.response.consume_event(timeout=self.queue_timeout)
         except EventTimeoutError as e:
             self.logger.error("Queue timed out")

{isar-1.32.3 → isar-1.33.0}/src/isar/state_machine/state_machine.py

@@ -22,6 +22,7 @@ from isar.state_machine.states.intervention_needed import InterventionNeeded
 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.recharging import Recharging
 from isar.state_machine.states.returning_home import ReturningHome
 from isar.state_machine.states.robot_standing_still import RobotStandingStill
 from isar.state_machine.states.stopping import Stopping
@@ -43,6 +44,7 @@ from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.telemetry.payloads import (
     InterventionNeededPayload,
+    MissionAbortedPayload,
     MissionPayload,
     RobotStatusPayload,
     TaskPayload,
@@ -108,6 +110,7 @@ class StateMachine(object):
         # Status states
         self.offline_state: State = Offline(self)
         self.blocked_protective_stopping_state: State = BlockedProtectiveStop(self)
+        self.recharging_state: State = Recharging(self)
 
         # Error and special status states
         self.unknown_status_state: State = UnknownStatus(self)
@@ -124,6 +127,7 @@ class StateMachine(object):
             self.blocked_protective_stopping_state,
             self.unknown_status_state,
             self.intervention_needed_state,
+            self.recharging_state,
         ]
 
         self.machine = Machine(
@@ -189,6 +193,12 @@ class StateMachine(object):
                 self.current_task = None
             self.send_task_status()
 
+    def battery_level_is_above_mission_start_threshold(self):
+        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)  # type: ignore
@@ -227,6 +237,32 @@ class StateMachine(object):
                 f"Task: {str(task.id)[:8]} was reported as task.status by the robot"
             )
 
+    def publish_mission_aborted(self, reason: str, can_be_continued: bool) -> None:
+        if not self.mqtt_publisher:
+            return
+
+        if self.current_mission is None:
+            self.logger.warning(
+                "Could not publish mission aborted message. No ongoing mission."
+            )
+            return
+
+        payload: MissionAbortedPayload = MissionAbortedPayload(
+            isar_id=settings.ISAR_ID,
+            robot_name=settings.ROBOT_NAME,
+            mission_id=self.current_mission.id,
+            reason=reason,
+            can_be_continued=can_be_continued,
+            timestamp=datetime.now(timezone.utc),
+        )
+
+        self.mqtt_publisher.publish(
+            topic=settings.TOPIC_ISAR_MISSION_ABORTED,
+            payload=json.dumps(payload, cls=EnhancedJSONEncoder),
+            qos=1,
+            retain=True,
+        )
+
     def publish_mission_status(self) -> None:
         if not self.mqtt_publisher:
             return
@@ -339,6 +375,8 @@ class StateMachine(object):
             return RobotStatus.BlockedProtectiveStop
         elif self.current_state == States.InterventionNeeded:
             return RobotStatus.InterventionNeeded
+        elif self.current_state == States.Recharging:
+            return RobotStatus.Recharging
         else:
             return RobotStatus.Busy
 

{isar-1.32.3 → isar-1.33.0}/src/isar/state_machine/states/await_next_mission.py

@@ -25,7 +25,9 @@ class AwaitNextMission(EventHandlerBase):
             EventHandlerMapping(
                 name="start_mission_event",
                 event=events.api_requests.start_mission.request,
-                handler=lambda event: start_mission_event_handler(state_machine, event),
+                handler=lambda event: start_mission_event_handler(
+                    state_machine, event, events.api_requests.start_mission.response
+                ),
             ),
             EventHandlerMapping(
                 name="return_home_event",

{isar-1.32.3 → isar-1.33.0}/src/isar/state_machine/states/home.py

@@ -23,7 +23,9 @@ class Home(EventHandlerBase):
             EventHandlerMapping(
                 name="start_mission_event",
                 event=events.api_requests.start_mission.request,
-                handler=lambda event: start_mission_event_handler(state_machine, event),
+                handler=lambda event: start_mission_event_handler(
+                    state_machine, event, events.api_requests.start_mission.response
+                ),
             ),
             EventHandlerMapping(
                 name="return_home_event",

{isar-1.32.3 → isar-1.33.0}/src/isar/state_machine/states/monitor.py

@@ -2,6 +2,7 @@ import logging
 from copy import deepcopy
 from typing import TYPE_CHECKING, Callable, List, Optional
 
+from isar.config.settings import settings
 from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
 from isar.models.events import Event
 from isar.services.utilities.threaded_request import ThreadedRequest
@@ -23,6 +24,7 @@ class Monitor(EventHandlerBase):
     def __init__(self, state_machine: "StateMachine"):
         logger = logging.getLogger("state_machine")
         events = state_machine.events
+        shared_state = state_machine.shared_state
 
         def _pause_mission_event_handler(event: Event[bool]) -> Optional[Callable]:
             if event.consume_event():
@@ -46,6 +48,23 @@ class Monitor(EventHandlerBase):
                 return state_machine.mission_finished  # type: ignore
             return None
 
+        def _robot_battery_level_updated_handler(
+            event: Event[float],
+        ) -> Optional[Callable]:
+            battery_level: float = event.check()
+            if battery_level < settings.ROBOT_MISSION_BATTERY_START_THRESHOLD:
+                state_machine.publish_mission_aborted(
+                    "Robot battery too low to continue mission", True
+                )
+                state_machine._finalize()
+                state_machine.logger.warning(
+                    "Cancelling current mission due to low battery"
+                )
+                state_machine.current_mission = None
+                state_machine.current_task = None
+                return state_machine.request_return_home  # type: ignore
+            return None
+
         event_handlers: List[EventHandlerMapping] = [
             EventHandlerMapping(
                 name="stop_mission_event",
@@ -85,6 +104,11 @@ class Monitor(EventHandlerBase):
                     state_machine, _handle_task_completed, event
                 ),
             ),
+            EventHandlerMapping(
+                name="robot_battery_update_event",
+                event=shared_state.robot_battery_level,
+                handler=_robot_battery_level_updated_handler,
+            ),
         ]
         super().__init__(
             state_name="monitor",