isar 1.33.6__tar.gz → 1.33.7__tar.gz

{isar-1.33.6 → isar-1.33.7}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: isar
-Version: 1.33.6
+Version: 1.33.7
 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.33.6 → isar-1.33.7}/src/isar/config/settings.py

@@ -59,6 +59,9 @@ class Settings(BaseSettings):
     # issues
     REQUEST_STATUS_COMMUNICATION_RECONNECT_DELAY: float = Field(default=10)
 
+    # Time allowed to clear the robot status before giving up
+    CLEAR_ROBOT_STATUS_TIMEOUT: int = Field(default=3)
+
     # Number of attempts for state transitions resume and pause if failed
     STATE_TRANSITION_NUM_RETIRES: int = Field(default=10)
 
@@ -78,6 +81,7 @@ class Settings(BaseSettings):
     ROBOT_STATUS_PUBLISH_INTERVAL: float = Field(default=1)
     ROBOT_HEARTBEAT_PUBLISH_INTERVAL: float = Field(default=1)
     ROBOT_INFO_PUBLISH_INTERVAL: float = Field(default=5)
+    ROBOT_API_BATTERY_POLL_INTERVAL: float = Field(default=5)
     ROBOT_API_STATUS_POLL_INTERVAL: float = Field(default=5)
     THREAD_CHECK_INTERVAL: float = Field(default=0.01)
 

{isar-1.33.6 → isar-1.33.7}/src/isar/models/events.py

@@ -123,6 +123,7 @@ class StateMachineEvents:
         self.stop_mission: Event[bool] = Event("stop_mission")
         self.pause_mission: Event[bool] = Event("pause_mission")
         self.task_status_request: Event[str] = Event("task_status_request")
+        self.clear_robot_status: Event[bool] = Event("clear_robot_status")
 
 
 class RobotServiceEvents:
@@ -132,6 +133,7 @@ class RobotServiceEvents:
         self.mission_started: Event[bool] = Event("mission_started")
         self.mission_failed: Event[ErrorMessage] = Event("mission_failed")
         self.robot_status_changed: Event[bool] = Event("robot_status_changed")
+        self.robot_status_cleared: Event[bool] = Event("robot_status_cleared")
         self.mission_failed_to_stop: Event[ErrorMessage] = Event(
             "mission_failed_to_stop"
         )

{isar-1.33.6 → isar-1.33.7}/src/isar/robot/robot.py

@@ -9,6 +9,7 @@ from isar.models.events import (
     SharedState,
     StateMachineEvents,
 )
+from isar.robot.robot_battery import RobotBatteryThread
 from isar.robot.robot_pause_mission import RobotPauseMissionThread
 from isar.robot.robot_start_mission import RobotStartMissionThread
 from isar.robot.robot_status import RobotStatusThread
@@ -29,6 +30,7 @@ class Robot(object):
         self.shared_state: SharedState = shared_state
         self.robot: RobotInterface = robot
         self.start_mission_thread: Optional[RobotStartMissionThread] = None
+        self.robot_battery_thread: Optional[RobotBatteryThread] = None
         self.robot_status_thread: Optional[RobotStatusThread] = None
         self.robot_task_status_thread: Optional[RobotTaskStatusThread] = None
         self.stop_mission_thread: Optional[RobotStopMissionThread] = None
@@ -39,6 +41,11 @@ class Robot(object):
         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_battery_thread is not None
+            and self.robot_battery_thread.is_alive()
+        ):
+            self.robot_battery_thread.join()
         if (
             self.robot_task_status_thread is not None
             and self.robot_task_status_thread.is_alive()
@@ -52,6 +59,7 @@ class Robot(object):
         if self.stop_mission_thread is not None and self.stop_mission_thread.is_alive():
             self.stop_mission_thread.join()
         self.robot_status_thread = None
+        self.robot_battery_thread = None
         self.robot_task_status_thread = None
         self.start_mission_thread = None
 
@@ -143,10 +151,19 @@ class Robot(object):
 
     def run(self) -> None:
         self.robot_status_thread = RobotStatusThread(
-            self.robot, self.signal_thread_quitting, self.shared_state
+            robot=self.robot,
+            signal_thread_quitting=self.signal_thread_quitting,
+            shared_state=self.shared_state,
+            state_machine_events=self.state_machine_events,
+            robot_service_events=self.robot_service_events,
         )
         self.robot_status_thread.start()
 
+        self.robot_battery_thread = RobotBatteryThread(
+            self.robot, self.signal_thread_quitting, self.shared_state
+        )
+        self.robot_battery_thread.start()
+
         while not self.signal_thread_quitting.wait(0):
             self._start_mission_event_handler(self.state_machine_events.start_mission)
 

isar-1.33.6/src/isar/robot/robot_status.py → isar-1.33.7/src/isar/robot/robot_battery.py

@@ -8,7 +8,7 @@ from robot_interface.models.exceptions.robot_exceptions import RobotException
 from robot_interface.robot_interface import RobotInterface
 
 
-class RobotStatusThread(Thread):
+class RobotBatteryThread(Thread):
     def __init__(
         self,
         robot: RobotInterface,
@@ -19,20 +19,24 @@ class RobotStatusThread(Thread):
         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() - settings.ROBOT_API_STATUS_POLL_INTERVAL
-        )
-        Thread.__init__(self, name="Robot status thread")
+        self.last_robot_battery_poll_time: float = time.time()
+        self.force_battery_poll_next_iteration: bool = True
+        Thread.__init__(self, name="Robot battery thread")
 
     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
+    def _is_ready_to_poll_for_battery(self) -> bool:
+        if self.force_battery_poll_next_iteration:
+            self.force_battery_poll_next_iteration = False
+            return True
+
+        time_since_last_robot_battery_poll = (
+            time.time() - self.last_robot_battery_poll_time
         )
         return (
-            time_since_last_robot_status_poll > settings.ROBOT_API_STATUS_POLL_INTERVAL
+            time_since_last_robot_battery_poll
+            > settings.ROBOT_API_BATTERY_POLL_INTERVAL
         )
 
     def run(self):
@@ -42,17 +46,15 @@ class RobotStatusThread(Thread):
         thread_check_interval = settings.THREAD_CHECK_INTERVAL
 
         while not self.signal_thread_quitting.wait(thread_check_interval):
-            if not self._is_ready_to_poll_for_status():
+            if not self._is_ready_to_poll_for_battery():
                 continue
             try:
-                self.last_robot_status_poll_time = time.time()
+                self.last_robot_battery_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}")
+                self.logger.error(f"Failed to retrieve robot battery level: {e}")
                 continue
-        self.logger.info("Exiting robot status thread")
+        self.logger.info("Exiting robot battery thread")

isar-1.33.7/src/isar/robot/robot_status.py

@@ -0,0 +1,71 @@
+import logging
+import time
+from threading import Event, Thread
+
+from isar.config.settings import settings
+from isar.models.events import RobotServiceEvents, SharedState, StateMachineEvents
+from robot_interface.models.exceptions.robot_exceptions import RobotException
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStatusThread(Thread):
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+        shared_state: SharedState,
+        robot_service_events: RobotServiceEvents,
+        state_machine_events: StateMachineEvents,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.shared_state: SharedState = shared_state
+        self.robot_service_events: RobotServiceEvents = robot_service_events
+        self.state_machine_events: StateMachineEvents = state_machine_events
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.robot_status_poll_interval: float = settings.ROBOT_API_STATUS_POLL_INTERVAL
+        self.last_robot_status_poll_time: float = time.time()
+        self.force_status_poll_next_iteration: bool = True
+        Thread.__init__(self, name="Robot status thread")
+
+    def stop(self) -> None:
+        return
+
+    def _is_ready_to_poll_for_status(self) -> bool:
+        if self.force_status_poll_next_iteration:
+            self.force_status_poll_next_iteration = False
+            return True
+
+        time_since_last_robot_status_poll = (
+            time.time() - self.last_robot_status_poll_time
+        )
+        return time_since_last_robot_status_poll > self.robot_status_poll_interval
+
+    def run(self):
+        if self.signal_thread_quitting.is_set():
+            return
+
+        thread_check_interval = settings.THREAD_CHECK_INTERVAL
+
+        while not self.signal_thread_quitting.wait(thread_check_interval):
+            if self.state_machine_events.clear_robot_status.consume_event() is not None:
+                self.shared_state.robot_status.clear_event()
+                self.robot_service_events.robot_status_changed.clear_event()
+                self.robot_service_events.robot_status_cleared.trigger_event(True)
+                self.force_status_poll_next_iteration = True
+
+            if not self._is_ready_to_poll_for_status():
+                continue
+
+            try:
+                self.last_robot_status_poll_time = time.time()
+
+                robot_status = self.robot.robot_status()
+
+                if robot_status is not self.shared_state.robot_status.check():
+                    self.shared_state.robot_status.update(robot_status)
+                    self.robot_service_events.robot_status_changed.trigger_event(True)
+            except RobotException as e:
+                self.logger.error(f"Failed to retrieve robot status: {e}")
+                continue
+        self.logger.info("Exiting robot status thread")

{isar-1.33.6 → isar-1.33.7}/src/isar/state_machine/states/blocked_protective_stop.py

@@ -1,7 +1,7 @@
 from typing import TYPE_CHECKING, List
 
 from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
-from isar.models.events import Event
+from isar.state_machine.utils.common_event_handlers import robot_status_event_handler
 from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
@@ -11,19 +11,19 @@ if TYPE_CHECKING:
 class BlockedProtectiveStop(EventHandlerBase):
 
     def __init__(self, state_machine: "StateMachine"):
+        events = state_machine.events
         shared_state = state_machine.shared_state
 
-        def _robot_status_event_handler(event: Event[RobotStatus]):
-            robot_status: RobotStatus = event.check()
-            if robot_status != RobotStatus.BlockedProtectiveStop:
-                return state_machine.robot_status_changed  # type: ignore
-            return None
-
         event_handlers: List[EventHandlerMapping] = [
             EventHandlerMapping(
                 name="robot_status_event",
-                event=shared_state.robot_status,
-                handler=_robot_status_event_handler,
+                event=events.robot_service_events.robot_status_changed,
+                handler=lambda event: robot_status_event_handler(
+                    state_machine=state_machine,
+                    expected_status=RobotStatus.BlockedProtectiveStop,
+                    status_changed_event=event,
+                    status_event=shared_state.robot_status,
+                ),
             ),
         ]
         super().__init__(

{isar-1.33.6 → isar-1.33.7}/src/isar/state_machine/states/home.py

@@ -20,17 +20,6 @@ class Home(EventHandlerBase):
         events = state_machine.events
         shared_state = state_machine.shared_state
 
-        def _robot_status_event_handler(
-            event: Event[RobotStatus],
-        ) -> Optional[Callable]:
-            robot_status: RobotStatus = event.check()
-            if not (
-                robot_status == RobotStatus.Available
-                or robot_status == RobotStatus.Home
-            ):
-                return state_machine.robot_status_changed  # type: ignore
-            return None
-
         def _send_to_lockdown_event_handler(event: Event[bool]):
             should_send_robot_home: bool = event.consume_event()
             if should_send_robot_home:
@@ -40,6 +29,21 @@ class Home(EventHandlerBase):
                 return state_machine.reached_lockdown  # type: ignore
             return None
 
+        def _robot_status_event_handler(
+            state_machine: "StateMachine",
+            status_changed_event: Event[bool],
+            status_event: Event[RobotStatus],
+        ) -> Optional[Callable]:
+            if not status_changed_event.consume_event():
+                return None
+            robot_status: Optional[RobotStatus] = status_event.check()
+            if not (
+                robot_status == RobotStatus.Available
+                or robot_status == RobotStatus.Home
+            ):
+                return state_machine.robot_status_changed  # type: ignore
+            return None
+
         event_handlers: List[EventHandlerMapping] = [
             EventHandlerMapping(
                 name="start_mission_event",
@@ -60,8 +64,12 @@ class Home(EventHandlerBase):
             ),
             EventHandlerMapping(
                 name="robot_status_event",
-                event=shared_state.robot_status,
-                handler=_robot_status_event_handler,
+                event=events.robot_service_events.robot_status_changed,
+                handler=lambda event: _robot_status_event_handler(
+                    state_machine=state_machine,
+                    status_changed_event=event,
+                    status_event=shared_state.robot_status,
+                ),
             ),
             EventHandlerMapping(
                 name="send_to_lockdown_event",

{isar-1.33.6 → isar-1.33.7}/src/isar/state_machine/states/offline.py

@@ -1,7 +1,7 @@
 from typing import TYPE_CHECKING, List
 
 from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
-from isar.models.events import Event
+from isar.state_machine.utils.common_event_handlers import robot_status_event_handler
 from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
@@ -11,20 +11,19 @@ if TYPE_CHECKING:
 class Offline(EventHandlerBase):
 
     def __init__(self, state_machine: "StateMachine"):
-
+        events = state_machine.events
         shared_state = state_machine.shared_state
 
-        def _robot_status_event_handler(event: Event[RobotStatus]):
-            robot_status: RobotStatus = event.check()
-            if robot_status != RobotStatus.Offline:
-                return state_machine.robot_status_changed  # type: ignore
-            return None
-
         event_handlers: List[EventHandlerMapping] = [
             EventHandlerMapping(
                 name="robot_status_event",
-                event=shared_state.robot_status,
-                handler=_robot_status_event_handler,
+                event=events.robot_service_events.robot_status_changed,
+                handler=lambda event: robot_status_event_handler(
+                    state_machine=state_machine,
+                    expected_status=RobotStatus.Offline,
+                    status_changed_event=event,
+                    status_event=shared_state.robot_status,
+                ),
             ),
         ]
         super().__init__(

{isar-1.33.6 → isar-1.33.7}/src/isar/state_machine/transitions/functions/robot_status.py

@@ -1,5 +1,7 @@
+import time
 from typing import TYPE_CHECKING
 
+from isar.config.settings import settings
 from robot_interface.models.mission.status import RobotStatus
 
 if TYPE_CHECKING:
@@ -19,3 +21,16 @@ def is_available_or_home(state_machine: "StateMachine") -> bool:
 def is_blocked_protective_stop(state_machine: "StateMachine") -> bool:
     robot_status = state_machine.shared_state.robot_status.check()
     return robot_status == RobotStatus.BlockedProtectiveStop
+
+
+def clear_robot_status(state_machine: "StateMachine") -> bool:
+    state_machine.events.state_machine_events.clear_robot_status.trigger_event(True)
+    start_time: float = time.time()
+    while time.time() - start_time < settings.CLEAR_ROBOT_STATUS_TIMEOUT:
+        if (
+            state_machine.events.robot_service_events.robot_status_cleared.consume_event()
+        ):
+            return True
+        time.sleep(settings.FSM_SLEEP_TIME)
+    state_machine.logger.error("Timed out waiting for robot status to be cleared")
+    return False

{isar-1.33.6 → isar-1.33.7}/src/isar/state_machine/transitions/return_home.py

@@ -12,6 +12,7 @@ from isar.state_machine.transitions.functions.return_home import (
     should_retry_return_home,
     start_return_home_mission,
 )
+from isar.state_machine.transitions.functions.robot_status import clear_robot_status
 from isar.state_machine.transitions.functions.start_mission import (
     initialize_robot,
     trigger_start_mission_event,
@@ -58,11 +59,19 @@ def get_return_home_transitions(state_machine: "StateMachine") -> List[dict]:
             "trigger": "returned_home",
             "source": state_machine.returning_home_state,
             "dest": state_machine.home_state,
+            "conditions": [
+                def_transition(state_machine, clear_robot_status),
+            ],
             "before": [
                 def_transition(state_machine, reset_return_home_failure_counter),
                 def_transition(state_machine, return_home_finished),
             ],
         },
+        {
+            "trigger": "returned_home",
+            "source": state_machine.returning_home_state,
+            "dest": state_machine.intervention_needed_state,
+        },
         {
             "trigger": "starting_recharging",
             "source": state_machine.returning_home_state,

{isar-1.33.6 → isar-1.33.7}/src/isar/state_machine/utils/common_event_handlers.py

@@ -43,9 +43,12 @@ def return_home_event_handler(
 def robot_status_event_handler(
     state_machine: "StateMachine",
     expected_status: RobotStatus,
-    event: Event[RobotStatus],
+    status_changed_event: Event[bool],
+    status_event: Event[RobotStatus],
 ) -> Optional[Callable]:
-    robot_status: RobotStatus = event.check()
+    if not status_changed_event.consume_event():
+        return None
+    robot_status: Optional[RobotStatus] = status_event.check()
     if robot_status != expected_status:
         return state_machine.robot_status_changed  # type: ignore
     return None

{isar-1.33.6 → isar-1.33.7}/src/isar.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: isar
-Version: 1.33.6
+Version: 1.33.7
 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.33.6 → isar-1.33.7}/src/isar.egg-info/SOURCES.txt

@@ -81,6 +81,7 @@ src/isar/mission_planner/task_selector_interface.py
 src/isar/models/__init__.py
 src/isar/models/events.py
 src/isar/robot/robot.py
+src/isar/robot/robot_battery.py
 src/isar/robot/robot_pause_mission.py
 src/isar/robot/robot_start_mission.py
 src/isar/robot/robot_status.py

{isar-1.33.6 → isar-1.33.7}/tests/isar/state_machine/test_state_machine.py

@@ -37,6 +37,7 @@ from tests.test_double.robot_interface import (
     StubRobot,
     StubRobotBlockedProtectiveStopToHomeTest,
     StubRobotOfflineToHomeTest,
+    StubRobotRobotStatusBusyIfNotHomeOrUnknownStatus,
 )
 from tests.test_double.task import StubTask
 
@@ -95,21 +96,34 @@ def test_state_machine_transitions_when_running_full_mission(
     container: ApplicationContainer,
     state_machine_thread: StateMachineThreadMock,
     robot_service_thread: RobotServiceThreadMock,
-    mocker,
 ) -> None:
     state_machine_thread.state_machine.await_next_mission_state.timers[
         0
     ].timeout_in_seconds = 0.01
+
+    robot_service_thread.robot_service.robot = (
+        StubRobotRobotStatusBusyIfNotHomeOrUnknownStatus(
+            current_state=robot_service_thread.robot_service.shared_state.state,
+            initiate_mission_delay=1,
+        )
+    )
     state_machine_thread.start()
-    mocker.patch.object(StubRobot, "robot_status", return_value=RobotStatus.Home)
     robot_service_thread.start()
+
+    # Setting the poll interval to a lower value to ensure that the robot status is
+    # updated during the mission. This value needs to be set after the robot service
+    # thread has been started.
+    robot_service_thread.robot_service.robot_status_thread.robot_status_poll_interval = (
+        0.5
+    )
+
     task_1: Task = TakeImage(
         target=DummyPose.default_pose().position, robot_pose=DummyPose.default_pose()
     )
     task_2: Task = TakeImage(
         target=DummyPose.default_pose().position, robot_pose=DummyPose.default_pose()
     )
-    mission: Mission = Mission(name="Dummy misson", tasks=[task_1, task_2])
+    mission: Mission = Mission(name="Dummy mission", tasks=[task_1, task_2])
 
     scheduling_utilities: SchedulingUtilities = container.scheduling_utilities()
     scheduling_utilities.start_mission(mission=mission)
@@ -965,7 +979,11 @@ def test_state_machine_with_return_home_failure_successful_retries(
     ].timeout_in_seconds = 0.01
     scheduling_utilities: SchedulingUtilities = container.scheduling_utilities()
     state_machine_thread.start()
-    mocker.patch.object(StubRobot, "robot_status", return_value=RobotStatus.Available)
+    mocker.patch.object(
+        StubRobot,
+        "robot_status",
+        side_effect=[RobotStatus.Available, RobotStatus.Home],
+    )
     mocker.patch.object(
         StubRobot, "task_status", side_effect=[TaskStatus.Failed, TaskStatus.Successful]
     )

{isar-1.33.6 → isar-1.33.7}/tests/test_double/robot_interface.py

@@ -1,7 +1,8 @@
+import time
 from datetime import datetime
 from queue import Queue
 from threading import Thread
-from typing import Callable, List
+from typing import Callable, List, Optional
 
 from alitra import Frame, Orientation, Pose, Position
 
@@ -33,14 +34,19 @@ class StubRobot(RobotInterface):
             frame=Frame("robot"),
         ),
         robot_status: RobotStatus = RobotStatus.Available,
+        initiate_mission_delay: float = 0.0,
     ):
         self.mission_status_return_value: MissionStatus = mission_status
         self.task_status_return_value: TaskStatus = task_status
         self.stop_return_value: bool = stop
         self.robot_pose_return_value: Pose = pose
         self.robot_status_return_value: RobotStatus = robot_status
+        self.initiate_mission_delay: float = initiate_mission_delay
+        self.mission: Optional[Mission] = None
 
     def initiate_mission(self, mission: Mission) -> None:
+        time.sleep(self.initiate_mission_delay)
+        self.mission = mission
         return
 
     def task_status(self, task_id: str) -> TaskStatus:
@@ -153,3 +159,34 @@ class StubRobotOfflineToHomeTest(StubRobot):
         if not self.entered_offline:
             return RobotStatus.Offline
         return RobotStatus.Home
+
+
+class StubRobotRobotStatusBusyIfNotHomeOrUnknownStatus(StubRobot):
+    def __init__(
+        self,
+        current_state: Event,
+        initiate_mission_delay: float = 0.0,
+    ):
+        super().__init__()
+        self.current_state = current_state
+        self.initiate_mission_delay: float = initiate_mission_delay
+        self.return_home_mission_just_finished_successfully = False
+
+    def task_status(self, task_id: str) -> TaskStatus:
+        if self.mission._is_return_to_home_mission():
+            if self.task_status_return_value == TaskStatus.Successful:
+                self.return_home_mission_just_finished_successfully = True
+        return self.task_status_return_value
+
+    def robot_status(self) -> RobotStatus:
+        current_state = self.current_state.check()
+        if current_state is None:
+            raise RobotCommunicationException("Could not read state machine state")
+        if current_state == "home":
+            return RobotStatus.Home
+        elif current_state == "unknown_status":
+            return RobotStatus.Home
+        elif self.return_home_mission_just_finished_successfully:
+            return RobotStatus.Home
+
+        return RobotStatus.Busy