isar 1.25.9__py3-none-any.whl → 1.26.1__py3-none-any.whl

isar/modules.py

@@ -16,7 +16,7 @@ from isar.mission_planner.local_planner import LocalPlanner
 from isar.mission_planner.mission_planner_interface import MissionPlannerInterface
 from isar.mission_planner.sequential_task_selector import SequentialTaskSelector
 from isar.mission_planner.task_selector_interface import TaskSelectorInterface
-from isar.models.communication.queues.queues import Queues
+from isar.models.communication.queues.events import Events, SharedState
 from isar.services.service_connections.request_handler import RequestHandler
 from isar.services.utilities.robot_utilities import RobotUtilities
 from isar.services.utilities.scheduling_utilities import SchedulingUtilities
@@ -76,11 +76,18 @@ class RobotModule(Module):
         return robot_interface.Robot()  # type: ignore
 
 
-class QueuesModule(Module):
+class EventsModule(Module):
     @provider
     @singleton
-    def provide_queues(self) -> Queues:
-        return Queues()
+    def provide_events(self) -> Events:
+        return Events()
+
+
+class SharedStateModule(Module):
+    @provider
+    @singleton
+    def provide_shared_state(self) -> SharedState:
+        return SharedState()
 
 
 class RequestHandlerModule(Module):
@@ -125,13 +132,15 @@ class StateMachineModule(Module):
     @singleton
     def provide_state_machine(
         self,
-        queues: Queues,
+        events: Events,
+        shared_state: SharedState,
         robot: RobotInterface,
         mqtt_client: MqttClientInterface,
         task_selector: TaskSelectorInterface,
     ) -> StateMachine:
         return StateMachine(
-            queues=queues,
+            events=events,
+            shared_state=shared_state,
             robot=robot,
             mqtt_publisher=mqtt_client,
             task_selector=task_selector,
@@ -143,12 +152,12 @@ class UploaderModule(Module):
     @singleton
     def provide_uploader(
         self,
-        queues: Queues,
+        events: Events,
         storage_handlers: List[StorageInterface],
         mqtt_client: MqttClientInterface,
     ) -> Uploader:
         return Uploader(
-            queues=queues,
+            events=events,
             storage_handlers=storage_handlers,
             mqtt_publisher=mqtt_client,
         )
@@ -158,9 +167,12 @@ class SchedulingUtilitiesModule(Module):
     @provider
     @singleton
     def provide_scheduling_utilities(
-        self, queues: Queues, mission_planner: MissionPlannerInterface
+        self,
+        events: Events,
+        shared_state: SharedState,
+        mission_planner: MissionPlannerInterface,
     ) -> SchedulingUtilities:
-        return SchedulingUtilities(queues, mission_planner)
+        return SchedulingUtilities(events, shared_state, mission_planner)
 
 
 class RobotUtilitiesModule(Module):
@@ -180,9 +192,9 @@ class ServiceModule(Module):
 class MqttModule(Module):
     @provider
     @singleton
-    def provide_mqtt_client(self, queues: Queues) -> MqttClientInterface:
+    def provide_mqtt_client(self, events: Events) -> MqttClientInterface:
         if settings.MQTT_ENABLED:
-            return MqttPublisher(mqtt_queue=queues.mqtt_queue)
+            return MqttPublisher(mqtt_queue=events.mqtt_queue)
         return None
 
 
@@ -196,7 +208,8 @@ class SequentialTaskSelectorModule(Module):
 modules: Dict[str, Tuple[Module, Union[str, bool]]] = {
     "api": (APIModule, "required"),
     "authentication": (AuthenticationModule, "required"),
-    "queues": (QueuesModule, "required"),
+    "events": (EventsModule, "required"),
+    "shared_state": (SharedStateModule, "required"),
     "request_handler": (RequestHandlerModule, "required"),
     "robot_package": (RobotModule, settings.ROBOT_PACKAGE),
     "isar_id": (RobotModule, settings.ISAR_ID),

isar/robot/robot.py

@@ -0,0 +1,124 @@
+import logging
+from queue import Queue
+from threading import Event
+from typing import Optional
+
+from injector import inject
+
+from isar.models.communication.queues.events import (
+    Events,
+    RobotServiceEvents,
+    SharedState,
+    StateMachineEvents,
+)
+from isar.models.communication.queues.queue_utils import check_for_event
+from isar.robot.robot_start_mission import RobotStartMissionThread
+from isar.robot.robot_status import RobotStatusThread
+from isar.robot.robot_stop_mission import RobotStopMissionThread
+from isar.robot.robot_task_status import RobotTaskStatusThread
+from robot_interface.robot_interface import RobotInterface
+
+
+class Robot(object):
+
+    @inject
+    def __init__(
+        self, events: Events, robot: RobotInterface, shared_state: SharedState
+    ):
+        self.logger = logging.getLogger("robot")
+        self.state_machine_events: StateMachineEvents = events.state_machine_events
+        self.robot_service_events: RobotServiceEvents = events.robot_service_events
+        self.shared_state: SharedState = shared_state
+        self.robot: RobotInterface = robot
+        self.start_mission_thread: Optional[RobotStartMissionThread] = None
+        self.robot_status_thread: Optional[RobotStatusThread] = None
+        self.robot_task_status_thread: Optional[RobotTaskStatusThread] = None
+        self.stop_mission_thread_thread: Optional[RobotStopMissionThread] = None
+        self.signal_thread_quitting: Event = Event()
+
+    def stop(self) -> None:
+        self.signal_thread_quitting.set()
+        if self.robot_status_thread is not None and self.robot_status_thread.is_alive():
+            self.robot_status_thread.join()
+        if (
+            self.robot_task_status_thread is not None
+            and self.robot_status_thread.is_alive()
+        ):
+            self.robot_task_status_thread.join()
+        if (
+            self.start_mission_thread is not None
+            and self.robot_status_thread.is_alive()
+        ):
+            self.start_mission_thread.join()
+        if (
+            self.stop_mission_thread_thread is not None
+            and self.stop_mission_thread_thread.is_alive()
+        ):
+            self.stop_mission_thread_thread.join()
+        self.robot_status_thread = None
+        self.robot_task_status_thread = None
+        self.start_mission_thread = None
+
+    def _check_and_handle_start_mission(self, event: Queue) -> None:
+        start_mission = check_for_event(event)
+        if start_mission is not None:
+            if (
+                self.start_mission_thread is not None
+                and self.start_mission_thread.is_alive()
+            ):
+                self.logger.warning(
+                    "Attempted to start mission while another mission was starting."
+                )
+                self.start_mission_thread.join()
+            self.start_mission_thread = RobotStartMissionThread(
+                self.robot_service_events,
+                self.robot,
+                self.signal_thread_quitting,
+                start_mission,
+            )
+            self.start_mission_thread.start()
+
+    def _check_and_handle_task_status_request(self, event: Queue[str]) -> None:
+        task_id: str = check_for_event(event)
+        if task_id:
+            self.robot_task_status_thread = RobotTaskStatusThread(
+                self.robot_service_events,
+                self.robot,
+                self.signal_thread_quitting,
+                task_id,
+            )
+            self.robot_task_status_thread.start()
+
+    def _check_and_handle_stop_mission(self, event: Queue) -> None:
+        if check_for_event(event):
+            if (
+                self.stop_mission_thread_thread is not None
+                and self.start_mission_thread.is_alive()
+            ):
+                return
+            self.stop_mission_thread_thread = RobotStopMissionThread(
+                self.robot_service_events, self.robot, self.signal_thread_quitting
+            )
+            self.stop_mission_thread_thread.start()
+
+    def run(self) -> None:
+        self.robot_status_thread = RobotStatusThread(
+            self.robot, self.signal_thread_quitting, self.shared_state
+        )
+        self.robot_status_thread.start()
+
+        while True:
+            if self.signal_thread_quitting.is_set():
+                break
+
+            self._check_and_handle_start_mission(
+                self.state_machine_events.start_mission
+            )
+
+            self._check_and_handle_task_status_request(
+                self.state_machine_events.task_status_request
+            )
+
+            self._check_and_handle_stop_mission(self.state_machine_events.stop_mission)
+
+        self.logger.info("Exiting robot service main thread")

isar/robot/robot_start_mission.py

@@ -0,0 +1,73 @@
+import logging
+from threading import Event, Thread
+
+from isar.config.settings import settings
+from isar.models.communication.queues.events import RobotServiceEvents
+from isar.models.communication.queues.queue_utils import (
+    trigger_event,
+    trigger_event_without_data,
+)
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    RobotException,
+    RobotInfeasibleMissionException,
+)
+from robot_interface.models.mission.mission import Mission
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStartMissionThread(Thread):
+
+    def __init__(
+        self,
+        robot_service_events: RobotServiceEvents,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+        mission: Mission,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot_service_events: RobotServiceEvents = robot_service_events
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.mission = mission
+        Thread.__init__(self, name="Robot start mission thread")
+
+    def run(self):
+        retries = 0
+        started_mission = False
+        try:
+            while not started_mission:
+                if self.signal_thread_quitting.wait(0):
+                    return
+                try:
+                    self.robot.initiate_mission(self.mission)
+                except RobotException as e:
+                    retries += 1
+                    self.logger.warning(
+                        f"Initiating failed #: {str(retries)} "
+                        f"because: {e.error_description}"
+                    )
+
+                    if retries >= settings.INITIATE_FAILURE_COUNTER_LIMIT:
+                        error_description = (
+                            f"Mission will be cancelled after failing to initiate "
+                            f"{settings.INITIATE_FAILURE_COUNTER_LIMIT} times because: "
+                            f"{e.error_description}"
+                        )
+
+                        trigger_event(
+                            self.robot_service_events.mission_failed,
+                            ErrorMessage(
+                                error_reason=e.error_reason,
+                                error_description=error_description,
+                            ),
+                        )
+                started_mission = True
+        except RobotInfeasibleMissionException as e:
+            trigger_event(
+                self.robot_service_events.mission_failed,
+                ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                ),
+            )
+        trigger_event_without_data(self.robot_service_events.mission_started)

isar/robot/robot_status.py

@@ -0,0 +1,49 @@
+import logging
+import time
+from threading import Event, Thread
+
+from isar.config.settings import settings
+from isar.models.communication.queues.events import SharedState
+from isar.models.communication.queues.queue_utils import update_shared_state
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStatusThread(Thread):
+
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+        shared_state: SharedState,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.shared_state: SharedState = shared_state
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.last_robot_status_poll_time: float = time.time()
+        Thread.__init__(self, name="Robot status thread", daemon=True)
+
+    def stop(self) -> None:
+        return
+
+    def _is_ready_to_poll_for_status(self) -> bool:
+        time_since_last_robot_status_poll = (
+            time.time() - self.last_robot_status_poll_time
+        )
+        return (
+            time_since_last_robot_status_poll > settings.ROBOT_API_STATUS_POLL_INTERVAL
+        )
+
+    def run(self):
+        while True:
+            if self.signal_thread_quitting.is_set():
+                break
+
+            if not self._is_ready_to_poll_for_status():
+                continue
+
+            robot_status = self.robot.robot_status()
+
+            update_shared_state(self.shared_state.robot_status, robot_status)
+            self.last_robot_status_poll_time = time.time()
+        self.logger.info("Exiting robot status thread")

isar/robot/robot_stop_mission.py

@@ -0,0 +1,72 @@
+import logging
+import time
+from threading import Event, Thread
+from typing import Optional
+
+from isar.config.settings import settings
+from isar.models.communication.queues.events import RobotServiceEvents
+from isar.models.communication.queues.queue_utils import (
+    trigger_event,
+    trigger_event_without_data,
+)
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    RobotActionException,
+    RobotException,
+)
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStopMissionThread(Thread):
+
+    def __init__(
+        self,
+        robot_service_events: RobotServiceEvents,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot_service_events: RobotServiceEvents = robot_service_events
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        Thread.__init__(self, name="Robot start mission thread")
+
+    def run(self) -> None:
+        retries = 0
+        error: Optional[ErrorMessage] = None
+        while retries < settings.STOP_ROBOT_ATTEMPTS_LIMIT:
+            if self.signal_thread_quitting.wait(0):
+                return
+
+            try:
+                self.robot.stop()
+            except (RobotActionException, RobotException) as e:
+                self.logger.warning(
+                    f"\nFailed to stop robot because: {e.error_description}"
+                    f"\nAttempting to stop the robot again"
+                )
+                retries += 1
+                error = ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                )
+                time.sleep(settings.FSM_SLEEP_TIME)
+                continue
+
+            trigger_event_without_data(
+                self.robot_service_events.mission_successfully_stopped
+            )
+            return
+
+        error_description = (
+            f"\nFailed to stop the robot after {retries} attempts because: "
+            f"{error.error_description}"
+            f"\nBe aware that the robot may still be moving even though a stop has "
+            "been attempted"
+        )
+
+        error_message = ErrorMessage(
+            error_reason=error.error_reason,
+            error_description=error_description,
+        )
+
+        trigger_event(self.robot_service_events.mission_failed_to_stop, error_message)

isar/robot/robot_task_status.py

@@ -0,0 +1,92 @@
+import logging
+import time
+from threading import Event, Thread
+from typing import Optional
+
+from isar.config.settings import settings
+from isar.models.communication.queues.events import RobotServiceEvents
+from isar.models.communication.queues.queue_utils import trigger_event
+from isar.services.utilities.threaded_request import ThreadedRequest
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    RobotCommunicationException,
+    RobotCommunicationTimeoutException,
+    RobotException,
+    RobotTaskStatusException,
+)
+from robot_interface.models.mission.status import TaskStatus
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotTaskStatusThread(Thread):
+
+    def __init__(
+        self,
+        robot_service_events: RobotServiceEvents,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+        task_id: str,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot_service_events: RobotServiceEvents = robot_service_events
+        self.robot: RobotInterface = robot
+        self.start_mission_thread: Optional[ThreadedRequest] = None
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.task_id: str = task_id
+        Thread.__init__(self, name="Robot task status thread")
+
+    def run(self) -> None:
+        task_status: TaskStatus = TaskStatus.NotStarted
+        failed_task_error: Optional[ErrorMessage] = None
+        request_status_failure_counter: int = 0
+
+        while (
+            request_status_failure_counter
+            < settings.REQUEST_STATUS_FAILURE_COUNTER_LIMIT
+        ):
+            if self.signal_thread_quitting.wait(0):
+                return
+            if request_status_failure_counter > 0:
+                time.sleep(settings.REQUEST_STATUS_COMMUNICATION_RECONNECT_DELAY)
+
+            try:
+                task_status = self.robot.task_status(self.task_id)
+                self.request_status_failure_counter = 0
+            except (
+                RobotCommunicationTimeoutException,
+                RobotCommunicationException,
+            ) as e:
+                self.request_status_failure_counter += 1
+                self.logger.error(
+                    f"Failed to get task status "
+                    f"{self.request_status_failure_counter} times because: "
+                    f"{e.error_description}"
+                )
+
+                failed_task_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                continue
+
+            except RobotTaskStatusException as e:
+                failed_task_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                break
+
+            except RobotException as e:
+                failed_task_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                break
+
+            trigger_event(self.robot_service_events.task_status_updated, task_status)
+            return
+
+        trigger_event(
+            self.robot_service_events.task_status_failed,
+            failed_task_error,
+        )

isar/script.py

@@ -12,8 +12,9 @@ from isar.apis.api import API
 from isar.config.keyvault.keyvault_service import Keyvault
 from isar.config.log import setup_loggers
 from isar.config.settings import robot_settings, settings
-from isar.models.communication.queues.queues import Queues
+from isar.models.communication.queues.events import Events
 from isar.modules import get_injector
+from isar.robot.robot import Robot
 from isar.services.service_connections.mqtt.mqtt_client import MqttClient
 from isar.services.service_connections.mqtt.robot_heartbeat_publisher import (
     RobotHeartbeatPublisher,
@@ -93,7 +94,8 @@ def start() -> None:
     state_machine: StateMachine = injector.get(StateMachine)
     uploader: Uploader = injector.get(Uploader)
     robot: RobotInterface = injector.get(RobotInterface)
-    queues: Queues = injector.get(Queues)
+    events: Events = injector.get(Events)
+    robot_service: Robot = injector.get(Robot)
 
     threads: List[Thread] = []
 
@@ -107,6 +109,11 @@ def start() -> None:
     )
     threads.append(uploader_thread)
 
+    robot_service_thread: Thread = Thread(
+        target=robot_service.run, name="Robot service", daemon=True
+    )
+    threads.append(robot_service_thread)
+
     if settings.UPLOAD_INSPECTIONS_ASYNC:
 
         def inspections_callback(inspection: Inspection, mission: Mission):
@@ -114,12 +121,12 @@ def start() -> None:
                 inspection,
                 mission,
             )
-            state_machine.queues.upload_queue.put(message)
+            state_machine.events.upload_queue.put(message)
 
         robot.register_inspection_callback(inspections_callback)
 
     if settings.MQTT_ENABLED:
-        mqtt_client: MqttClient = MqttClient(mqtt_queue=queues.mqtt_queue)
+        mqtt_client: MqttClient = MqttClient(mqtt_queue=events.mqtt_queue)
 
         mqtt_thread: Thread = Thread(
             target=mqtt_client.run, name="ISAR MQTT Client", daemon=True
@@ -127,7 +134,7 @@ def start() -> None:
         threads.append(mqtt_thread)
 
         robot_info_publisher: RobotInfoPublisher = RobotInfoPublisher(
-            mqtt_queue=queues.mqtt_queue
+            mqtt_queue=events.mqtt_queue
         )
         robot_info_thread: Thread = Thread(
             target=robot_info_publisher.run,
@@ -137,7 +144,7 @@ def start() -> None:
         threads.append(robot_info_thread)
 
         robot_heartbeat_publisher: RobotHeartbeatPublisher = RobotHeartbeatPublisher(
-            mqtt_queue=queues.mqtt_queue
+            mqtt_queue=events.mqtt_queue
         )
 
         robot_heartbeat_thread: Thread = Thread(
@@ -148,7 +155,7 @@ def start() -> None:
         threads.append(robot_heartbeat_thread)
 
         publishers: List[Thread] = robot.get_telemetry_publishers(
-            queue=queues.mqtt_queue,
+            queue=events.mqtt_queue,
             robot_name=settings.ROBOT_NAME,
             isar_id=settings.ISAR_ID,
         )