Report step status for missionwise

src/isar/state_machine/state_machine.py

@@ -12,7 +12,7 @@
 from transitions.core import State
 
 from isar.apis.models.models import ControlMissionResponse
-from isar.config.settings import settings
+from isar.config.settings import robot_settings, settings
 from isar.mission_planner.task_selector_interface import (
     TaskSelectorInterface,
     TaskSelectorStop,
@@ -233,6 +233,9 @@ def _initiated(self) -> None:
                 f"step: {str(self.current_step.id)[:8]}"
             )
         else:
+            if "step_status" in robot_settings.CAPABILITIES:
+                self.current_step.status = StepStatus.InProgress
+                self.publish_step_status(step=self.current_step)
             self.current_mission.status = MissionStatus.InProgress
             self.logger.info(
                 f"Successfully initiated full mission with ID: "

src/isar/state_machine/states/monitor.py

@@ -6,6 +6,8 @@
 from injector import inject
 from transitions import State
 
+from isar.config.settings import robot_settings, settings
+from isar.mission_planner.task_selector_interface import TaskSelectorStop
 from isar.services.utilities.threaded_request import (
     ThreadedRequest,
     ThreadedRequestNotFinishedError,
@@ -19,7 +21,7 @@
 )
 from robot_interface.models.inspection.inspection import Inspection
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus
+from robot_interface.models.mission.status import MissionStatus, TaskStatus
 from robot_interface.models.mission.step import InspectionStep, Step, StepStatus
 
 if TYPE_CHECKING:
@@ -56,7 +58,10 @@ def _run(self) -> None:
                 break
 
             if not self.step_status_thread:
-                if self.state_machine.stepwise_mission:
+                if (
+                    self.state_machine.stepwise_mission
+                    or "step_status" in robot_settings.CAPABILITIES
+                ):
                     self._run_get_status_thread(
                         status_function=self.state_machine.robot.step_status,
                         thread_name="State Machine Monitor Get Step Status",
@@ -97,7 +102,10 @@ def _run(self) -> None:
 
             except RobotException as e:
                 self._set_error_message(e)
-                if self.state_machine.stepwise_mission:
+                if (
+                    self.state_machine.stepwise_mission
+                    or "step_status" in robot_settings.CAPABILITIES
+                ):
                     status = StepStatus.Failed
                 else:
                     status = MissionStatus.Failed
@@ -106,7 +114,7 @@ def _run(self) -> None:
                     f"Retrieving the status failed because: {e.error_description}"
                 )
 
-            if self.state_machine.stepwise_mission and isinstance(status, StepStatus):
+            if isinstance(status, StepStatus):
                 self.state_machine.current_step.status = status
             elif isinstance(status, MissionStatus):
                 self.state_machine.current_mission.status = status
@@ -126,7 +134,30 @@ def _run(self) -> None:
                     transition = self.state_machine.step_finished  # type: ignore
                     break
             else:
-                if self._mission_finished(self.state_machine.current_mission):
+                if (
+                    isinstance(status, StepStatus)
+                    and "step_status" in robot_settings.CAPABILITIES
+                ):
+                    if (
+                        status != StepStatus.InProgress
+                        and status != StepStatus.NotStarted
+                    ):  # If task is done
+                        self.state_machine.current_task.update_task_status()
+                    else:  # If not all steps are done
+                        self.state_machine.current_task.status = TaskStatus.InProgress
+
+                    self.state_machine.publish_task_status(
+                        self.state_machine.current_task
+                    )
+                    if self.state_machine.current_task.status == TaskStatus.Successful:
+                        try:
+                            self.state_machine.current_task = (
+                                self.state_machine.task_selector.next_task()
+                            )
+                        except TaskSelectorStop:
+                            # Indicates that all tasks are finished
+                            self.state_machine.current_task = None
+                elif self._mission_finished(self.state_machine.current_mission):
                     transition = self.state_machine.full_mission_finished  # type: ignore
                     break
 
@@ -196,18 +227,33 @@ def _mission_finished(mission: Mission) -> bool:
         return False
 
     def _should_upload_inspections(self) -> bool:
-        step: Step = self.state_machine.current_step
-        return (
-            self._step_finished(step)
-            and step.status == StepStatus.Successful
-            and isinstance(step, InspectionStep)
-        )
+        if (
+            self.state_machine.stepwise_mission
+            or "step_status" in robot_settings.CAPABILITIES
+        ):
+            step: Step = self.state_machine.current_step
+            return (
+                self._step_finished(step)
+                and step.status == StepStatus.Successful
+                and isinstance(step, InspectionStep)
+            )
+        else:
+            mission_status: MissionStatus = self.state_machine.current_mission.status
+            if (
+                mission_status == MissionStatus.Successful
+                or mission_status == MissionStatus.PartiallySuccessful
+            ):
+                return True
+            return False
 
     def _set_error_message(self, e: RobotException) -> None:
         error_message: ErrorMessage = ErrorMessage(
             error_reason=e.error_reason, error_description=e.error_description
         )
-        if self.state_machine.stepwise_mission:
+        if (
+            self.state_machine.stepwise_mission
+            or "step_status" in robot_settings.CAPABILITIES
+        ):
             self.state_machine.current_step.error_message = error_message
         else:
             if self.state_machine.current_mission:

tests/isar/state_machine/states/test_monitor.py

@@ -2,7 +2,7 @@
 
 from isar.state_machine.states.monitor import Monitor
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import StepStatus
+from robot_interface.models.mission.status import MissionStatus, StepStatus
 from robot_interface.models.mission.step import Step, TakeImage
 from robot_interface.models.mission.task import Task
 from tests.mocks.step import MockStep
@@ -27,19 +27,20 @@ def test_step_finished(monitor: Monitor, mock_status, expected_output):
 
 
 @pytest.mark.parametrize(
-    "mock_status, should_queue_upload",
+    "successful, should_queue_upload",
     [
-        (StepStatus.Successful, True),
-        (StepStatus.Failed, False),
+        (True, True),
+        (False, False),
     ],
 )
 def test_should_only_upload_if_status_is_completed(
-    monitor: Monitor, mock_status, should_queue_upload
+    monitor: Monitor, successful, should_queue_upload
 ):
     step: TakeImage = MockStep.take_image_in_coordinate_direction()
-    step.status = mock_status
+    step.status = StepStatus.Successful if successful else StepStatus.Failed
     task: Task = Task(steps=[step])
     mission: Mission = Mission(tasks=[task])
+    mission.status = MissionStatus.Successful if successful else MissionStatus.Failed
 
     monitor.state_machine.current_mission = mission
     monitor.state_machine.current_task = task

tests/isar/state_machine/test_state_machine.py

@@ -90,27 +90,50 @@ def test_reset_state_machine(state_machine) -> None:
 empty_mission: Mission = Mission([], None)
 
 
-def test_state_machine_transitions(injector, state_machine_thread) -> None:
+@pytest.mark.parametrize(
+    "should_run_stepwise",
+    [
+        (True),
+        (False),
+    ],
+)
+def test_state_machine_transitions(
+    injector, state_machine_thread, should_run_stepwise
+) -> None:
     step_1: Step = DriveToPose(pose=MockPose.default_pose())
     step_2: Step = TakeImage(target=MockPose.default_pose().position)
     mission: Mission = Mission(tasks=[Task(steps=[step_1, step_2])])  # type: ignore
 
+    state_machine_thread.state_machine.stepwise_mission = should_run_stepwise
+
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
     scheduling_utilities.start_mission(mission=mission, initial_pose=None)
 
     time.sleep(3)
-    expected_transitions_list = deque(
-        [
-            States.Idle,
-            States.Initialize,
-            States.Initiate,
-            States.Monitor,
-            States.Initiate,
-            States.Monitor,
-            States.Initiate,
-            States.Idle,
-        ]
-    )
+    if should_run_stepwise:
+        expected_transitions_list = deque(
+            [
+                States.Idle,
+                States.Initialize,
+                States.Initiate,
+                States.Monitor,
+                States.Initiate,
+                States.Monitor,
+                States.Initiate,
+                States.Idle,
+            ]
+        )
+    else:
+        expected_transitions_list = deque(
+            [
+                States.Idle,
+                States.Initialize,
+                States.Initiate,
+                States.Monitor,
+                States.Initiate,
+                States.Idle,
+            ]
+        )
     assert (
         state_machine_thread.state_machine.transitions_list == expected_transitions_list
     )