diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 71422dfe..d9c00ba2 100644 --- a/src/isar/state_machine/state_machine.py +++ b/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: " diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index fff33ca6..fbf3d31a 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/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: diff --git a/tests/isar/state_machine/states/test_monitor.py b/tests/isar/state_machine/states/test_monitor.py index 929e1ed1..0e5d673d 100644 --- a/tests/isar/state_machine/states/test_monitor.py +++ b/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 diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index e3a06092..800744a0 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/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 )