From a9519c1cc4d6b785da626e900650deabef550ffc Mon Sep 17 00:00:00 2001 From: oysand Date: Wed, 19 Feb 2025 13:47:46 +0100 Subject: [PATCH] Add return home --- .../apis/models/start_mission_definition.py | 12 +- .../apis/schedule/scheduling_controller.py | 21 +- src/isar/config/settings.py | 3 + .../utilities/scheduling_utilities.py | 16 +- src/isar/state_machine/state_machine.py | 71 ++++++- .../states/await_next_mission.py | 56 ++++++ src/isar/state_machine/states/docked.py | 91 +++++++++ src/isar/state_machine/states/idle.py | 4 + src/isar/state_machine/states/initiate.py | 11 ++ .../state_machine/states/returning_home.py | 185 ++++++++++++++++++ src/isar/state_machine/states_enum.py | 3 + src/robot_interface/models/mission/status.py | 1 + src/robot_interface/models/mission/task.py | 1 - tests/isar/mission/test_mission.py | 8 +- .../services/readers/test_mission_reader.py | 29 +-- .../isar/state_machine/test_state_machine.py | 68 ++++++- tests/mocks/robot_interface.py | 24 +++ tests/mocks/task.py | 3 +- tests/test_data/test_mission_working.json | 37 +--- 19 files changed, 528 insertions(+), 116 deletions(-) create mode 100644 src/isar/state_machine/states/await_next_mission.py create mode 100644 src/isar/state_machine/states/docked.py create mode 100644 src/isar/state_machine/states/returning_home.py diff --git a/src/isar/apis/models/start_mission_definition.py b/src/isar/apis/models/start_mission_definition.py index b2a6222e..1814c591 100644 --- a/src/isar/apis/models/start_mission_definition.py +++ b/src/isar/apis/models/start_mission_definition.py @@ -61,7 +61,6 @@ class StartMissionDefinition(BaseModel): def to_isar_mission( start_mission_definition: StartMissionDefinition, - return_pose: Optional[InputPose] = None, ) -> Mission: isar_tasks: List[TASKS] = [] @@ -69,9 +68,6 @@ def to_isar_mission( task: TASKS = to_isar_task(task_definition) isar_tasks.append(task) - if return_pose: - isar_tasks.append(ReturnToHome(pose=return_pose.to_alitra_pose())) - if not isar_tasks: raise MissionPlannerError("Mission does not contain any valid tasks") @@ -96,7 +92,7 @@ def to_isar_task(task_definition: StartMissionTaskDefinition) -> TASKS: if task_definition.type == TaskType.Inspection: return to_inspection_task(task_definition) elif task_definition.type == TaskType.ReturnToHome: - return create_return_to_home_task(task_definition) + return ReturnToHome() else: raise MissionPlannerError( f"Failed to create task: '{task_definition.type}' is not a valid" @@ -166,11 +162,5 @@ def to_inspection_task(task_definition: StartMissionTaskDefinition) -> TASKS: ) -def create_return_to_home_task( - task_definition: StartMissionTaskDefinition, -) -> ReturnToHome: - return ReturnToHome(pose=task_definition.pose.to_alitra_pose()) - - def _build_mission_name() -> str: return f"{settings.PLANT_SHORT_NAME}{settings.ROBOT_NAME}{int(time.time())}" diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py index d53c9ef9..e09441b5 100644 --- a/src/isar/apis/schedule/scheduling_controller.py +++ b/src/isar/apis/schedule/scheduling_controller.py @@ -1,15 +1,12 @@ import logging from http import HTTPStatus -from typing import Optional -from alitra import Pose from fastapi import Body, HTTPException, Path from injector import inject from isar.apis.models.models import ( ControlMissionResponse, TaskResponse, - InputPose, StartMissionResponse, ) from isar.apis.models.start_mission_definition import ( @@ -25,7 +22,6 @@ TASKS, InspectionTask, MoveArm, - ReturnToHome, ) @@ -45,12 +41,6 @@ def start_mission_by_id( title="Mission ID", description="ID-number for predefined mission", ), - return_pose: Optional[InputPose] = Body( - default=None, - description="End pose of the mission. The robot return to the specified " - "pose after finishing all inspections", - embed=True, - ), ) -> StartMissionResponse: self.logger.info(f"Received request to start mission with id {mission_id}") @@ -58,9 +48,6 @@ def start_mission_by_id( self.scheduling_utilities.verify_state_machine_ready_to_receive_mission(state) mission: Mission = self.scheduling_utilities.get_mission(mission_id) - if return_pose: - pose: Pose = return_pose.to_alitra_pose() - mission.tasks.append(ReturnToHome(pose=pose)) self.scheduling_utilities.verify_robot_capable_of_mission( mission=mission, robot_capabilities=robot_settings.CAPABILITIES @@ -80,12 +67,6 @@ def start_mission( title="Mission Definition", description="Description of the mission in json format", ), - return_pose: Optional[InputPose] = Body( - default=None, - description="End pose of the mission. The robot return to the specified " - "pose after finishing all inspections", - embed=True, - ), ) -> StartMissionResponse: self.logger.info("Received request to start new mission") @@ -103,7 +84,7 @@ def start_mission( try: mission: Mission = to_isar_mission( - start_mission_definition=mission_definition, return_pose=return_pose + start_mission_definition=mission_definition ) except MissionPlannerError as e: error_message = f"Bad Request - Cannot create ISAR mission: {e}" diff --git a/src/isar/config/settings.py b/src/isar/config/settings.py index ae1b6a42..19bc79e9 100644 --- a/src/isar/config/settings.py +++ b/src/isar/config/settings.py @@ -82,6 +82,9 @@ def __init__(self) -> None: # FastAPI port API_PORT: int = Field(default=3000) + # Determines how long delay time should be allowed before returning home + RETURN_HOME_DELAY: int = Field(default=10) + # Determines which mission planner module is used by ISAR MISSION_PLANNER: str = Field(default="local") diff --git a/src/isar/services/utilities/scheduling_utilities.py b/src/isar/services/utilities/scheduling_utilities.py index 2b1937ac..180dea72 100644 --- a/src/isar/services/utilities/scheduling_utilities.py +++ b/src/isar/services/utilities/scheduling_utilities.py @@ -126,12 +126,16 @@ def verify_state_machine_ready_to_receive_mission(self, state: States) -> bool: HTTPException 409 Conflict If state machine is not idle and therefore can not start a new mission """ - if state != States.Idle: - error_message = f"Conflict - Robot is not idle - State: {state}" - self.logger.warning(error_message) - raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message) - - return True + if ( + state == States.Docked + or state == States.Idle + or state == States.AwaitNextMission + ): + return True + + error_message = f"Conflict - Robot is not idle - State: {state}" + self.logger.warning(error_message) + raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message) def start_mission( self, diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 010394b3..d75be75e 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -17,10 +17,13 @@ ) from isar.models.communication.message import StartMissionMessage from isar.models.communication.queues.queues import Queues +from isar.state_machine.states.docked import Docked from isar.state_machine.states.idle import Idle from isar.state_machine.states.initialize import Initialize from isar.state_machine.states.initiate import Initiate from isar.state_machine.states.monitor import Monitor +from isar.state_machine.states.returning_home import ReturningHome +from isar.state_machine.states.await_next_mission import AwaitNextMission from isar.state_machine.states.off import Off from isar.state_machine.states.offline import Offline from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop @@ -30,7 +33,7 @@ from robot_interface.models.exceptions.robot_exceptions import ErrorMessage from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.status import MissionStatus, RobotStatus, TaskStatus -from robot_interface.models.mission.task import TASKS +from robot_interface.models.mission.task import TASKS, ReturnToHome from robot_interface.robot_interface import RobotInterface from robot_interface.telemetry.mqtt_client import MqttClientInterface from robot_interface.telemetry.payloads import ( @@ -83,9 +86,12 @@ def __init__( # List of states self.stop_state: State = Stop(self) self.paused_state: State = Paused(self) + self.docked_state: State = Docked(self) self.idle_state: State = Idle(self) self.initialize_state: State = Initialize(self) self.monitor_state: State = Monitor(self) + self.returning_home_state: State = ReturningHome(self) + self.await_next_mission_state: State = AwaitNextMission(self) self.initiate_state: State = Initiate(self) self.off_state: State = Off(self) self.offline_state: State = Offline(self) @@ -93,10 +99,13 @@ def __init__( self.states: List[State] = [ self.off_state, + self.docked_state, self.idle_state, self.initialize_state, self.initiate_state, self.monitor_state, + self.returning_home_state, + self.await_next_mission_state, self.stop_state, self.paused_state, self.offline_state, @@ -126,9 +135,12 @@ def __init__( { "trigger": "stop", "source": [ + self.docked_state, self.idle_state, self.initiate_state, self.monitor_state, + self.returning_home_state, + self.await_next_mission_state, self.paused_state, ], "dest": self.stop_state, @@ -136,15 +148,36 @@ def __init__( { "trigger": "mission_finished", "source": self.monitor_state, - "dest": self.idle_state, + "dest": self.await_next_mission_state, "before": self._mission_finished, }, { "trigger": "mission_started", - "source": self.idle_state, + "source": [ + self.docked_state, + self.idle_state, + self.await_next_mission_state, + ], "dest": self.initialize_state, "before": self._mission_started, }, + { + "trigger": "return_home", + "source": self.await_next_mission_state, + "dest": self.initialize_state, + "before": self._return_home, + }, + { + "trigger": "return_home_initiated", + "source": self.initiate_state, + "dest": self.returning_home_state, + }, + { + "trigger": "return_home_finished", + "source": self.returning_home_state, + "dest": self.idle_state, + "before": self._return_home_finished, + }, { "trigger": "initialization_successful", "source": self.initialize_state, @@ -185,6 +218,16 @@ def __init__( "source": self.offline_state, "dest": self.idle_state, }, + { + "trigger": "robot_docked", + "source": self.idle_state, + "dest": self.docked_state, + }, + { + "trigger": "robot_undocked", + "source": self.docked_state, + "dest": self.idle_state, + }, { "trigger": "robot_protective_stop_engaged", "source": self.idle_state, @@ -285,6 +328,22 @@ def _mission_started(self) -> None: self.current_task.status = TaskStatus.InProgress self.publish_task_status(task=self.current_task) + def _return_home(self) -> None: + self.start_mission( + Mission( + tasks=[ReturnToHome()], + name="Return Home", + ) + ) + self.log_mission_overview(mission=self.current_mission) + self.current_mission.status = MissionStatus.InProgress + + self.current_task = self.task_selector.next_task() + self.current_task.status = TaskStatus.InProgress + + def _return_home_finished(self) -> None: + self.reset_state_machine() + def _full_mission_finished(self) -> None: self.current_task = None @@ -485,7 +544,11 @@ def publish_status(self) -> None: ) def _current_status(self) -> RobotStatus: - if self.current_state == States.Idle: + if ( + self.current_state == States.Docked + or self.current_state == States.Idle + or self.current_state == States.AwaitNextMission + ): return RobotStatus.Available elif self.current_state == States.Offline: return RobotStatus.Offline diff --git a/src/isar/state_machine/states/await_next_mission.py b/src/isar/state_machine/states/await_next_mission.py new file mode 100644 index 00000000..d6bf87f8 --- /dev/null +++ b/src/isar/state_machine/states/await_next_mission.py @@ -0,0 +1,56 @@ +import logging +import time +from typing import TYPE_CHECKING, Optional + +from transitions import State + +from isar.config.settings import settings +from isar.models.communication.message import StartMissionMessage + +if TYPE_CHECKING: + from isar.state_machine.state_machine import StateMachine + + +class AwaitNextMission(State): + def __init__(self, state_machine: "StateMachine") -> None: + super().__init__( + name="await_next_mission", on_enter=self.start, on_exit=self.stop + ) + self.state_machine: "StateMachine" = state_machine + self.logger = logging.getLogger("state_machine") + self.entered_time: float = time.time() + self.return_home_delay: float = settings.RETURN_HOME_DELAY + + def start(self) -> None: + self.state_machine.update_state() + self.entered_time = time.time() + self._run() + + def stop(self) -> None: + pass + + def _should_return_home(self) -> bool: + time_since_entered = time.time() - self.entered_time + return time_since_entered > self.return_home_delay + + def _run(self) -> None: + while True: + if self.state_machine.should_stop_mission(): + transition = self.state_machine.stop # type: ignore + break + + start_mission: Optional[StartMissionMessage] = ( + self.state_machine.should_start_mission() + ) + if start_mission: + self.state_machine.start_mission(mission=start_mission.mission) + transition = self.state_machine.mission_started # type: ignore + break + + if self._should_return_home(): + transition = self.state_machine.return_home # type: ignore + break + + time.sleep(self.state_machine.sleep_time) + + transition() diff --git a/src/isar/state_machine/states/docked.py b/src/isar/state_machine/states/docked.py new file mode 100644 index 00000000..8930032d --- /dev/null +++ b/src/isar/state_machine/states/docked.py @@ -0,0 +1,91 @@ +import logging +import time +from typing import TYPE_CHECKING, Optional + +from transitions import State + +from isar.config.settings import settings +from isar.models.communication.message import StartMissionMessage +from isar.services.utilities.threaded_request import ( + ThreadedRequest, + ThreadedRequestNotFinishedError, +) +from robot_interface.models.exceptions.robot_exceptions import RobotException +from robot_interface.models.mission.status import RobotStatus + +if TYPE_CHECKING: + from isar.state_machine.state_machine import StateMachine + + +class Docked(State): + def __init__(self, state_machine: "StateMachine") -> None: + super().__init__(name="docked", on_enter=self.start, on_exit=self.stop) + self.state_machine: "StateMachine" = state_machine + self.logger = logging.getLogger("state_machine") + self.robot_status_thread: Optional[ThreadedRequest] = None + self.last_robot_status_poll_time: float = time.time() + + def start(self) -> None: + self.state_machine.update_state() + self._run() + + def stop(self) -> None: + if self.robot_status_thread: + self.robot_status_thread.wait_for_thread() + self.robot_status_thread = None + + 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) -> None: + while True: + if self.state_machine.should_stop_mission(): + transition = self.state_machine.stop # type: ignore + break + + start_mission: Optional[StartMissionMessage] = ( + self.state_machine.should_start_mission() + ) + if start_mission: + self.state_machine.start_mission(mission=start_mission.mission) + transition = self.state_machine.mission_started # type: ignore + break + + time.sleep(self.state_machine.sleep_time) + + if not self._is_ready_to_poll_for_status(): + continue + + if not self.robot_status_thread: + self.robot_status_thread = ThreadedRequest( + request_func=self.state_machine.robot.robot_status + ) + self.robot_status_thread.start_thread( + name="State Machine Offline Get Robot Status" + ) + + try: + robot_status: RobotStatus = self.robot_status_thread.get_output() + except ThreadedRequestNotFinishedError: + time.sleep(self.state_machine.sleep_time) + continue + + except RobotException as e: + self.logger.error( + f"Failed to get robot status because: {e.error_description}" + ) + + self.last_robot_status_poll_time = time.time() + + if robot_status != RobotStatus.Docked: + transition = self.state_machine.robot_undocked # type: ignore + break + + self.robot_status_thread = None + + transition() diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py index dca9df46..74594b8b 100644 --- a/src/isar/state_machine/states/idle.py +++ b/src/isar/state_machine/states/idle.py @@ -61,6 +61,7 @@ def _run(self) -> None: self.state_machine.start_mission(mission=start_mission.mission) transition = self.state_machine.mission_started # type: ignore break + time.sleep(self.state_machine.sleep_time) if not self._is_ready_to_poll_for_status(): @@ -94,6 +95,9 @@ def _run(self) -> None: elif robot_status == RobotStatus.BlockedProtectiveStop: transition = self.state_machine.robot_protective_stop_engaged # type: ignore break + elif robot_status == RobotStatus.Docked: + transition = self.state_machine.robot_docked # type: ignore + break self.robot_status_thread = None diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py index 36bbb18d..e23fcb7a 100644 --- a/src/isar/state_machine/states/initiate.py +++ b/src/isar/state_machine/states/initiate.py @@ -14,6 +14,7 @@ RobotException, RobotInfeasibleMissionException, ) +from robot_interface.models.mission.task import ReturnToHome if TYPE_CHECKING: from isar.state_machine.state_machine import StateMachine @@ -41,6 +42,12 @@ def stop(self) -> None: self.initiate_thread.wait_for_thread() self.initiate_thread = None + def _is_return_to_home_mission(self) -> bool: + if len(self.state_machine.current_mission.tasks) > 1: + return False + + return isinstance(self.state_machine.current_mission.tasks[0], ReturnToHome) + def _run(self) -> None: transition: Callable while True: @@ -57,6 +64,10 @@ def _run(self) -> None: try: self.initiate_thread.get_output() + if self._is_return_to_home_mission(): + transition = self.state_machine.return_home_initiated + break + transition = self.state_machine.initiated # type: ignore break except ThreadedRequestNotFinishedError: diff --git a/src/isar/state_machine/states/returning_home.py b/src/isar/state_machine/states/returning_home.py new file mode 100644 index 00000000..5bfa929a --- /dev/null +++ b/src/isar/state_machine/states/returning_home.py @@ -0,0 +1,185 @@ +import logging +import time +from typing import TYPE_CHECKING, Callable, Optional, Union + +from injector import inject +from transitions import State + +from isar.config.settings import settings +from isar.services.utilities.threaded_request import ( + ThreadedRequest, + ThreadedRequestNotFinishedError, +) +from robot_interface.models.exceptions.robot_exceptions import ( + ErrorMessage, + RobotCommunicationException, + RobotCommunicationTimeoutException, + RobotException, + RobotTaskStatusException, +) +from robot_interface.models.mission.status import TaskStatus +from robot_interface.models.mission.task import Task + +if TYPE_CHECKING: + from isar.state_machine.state_machine import StateMachine + + +class ReturningHome(State): + @inject + def __init__(self, state_machine: "StateMachine") -> None: + super().__init__(name="returning_home", on_enter=self.start, on_exit=self.stop) + self.state_machine: "StateMachine" = state_machine + self.request_status_failure_counter: int = 0 + self.request_status_failure_counter_limit: int = ( + settings.REQUEST_STATUS_FAILURE_COUNTER_LIMIT + ) + + self.logger = logging.getLogger("state_machine") + self.task_status_thread: Optional[ThreadedRequest] = None + + def start(self) -> None: + self.state_machine.update_state() + self._run() + + def stop(self) -> None: + if self.task_status_thread: + self.task_status_thread.wait_for_thread() + self.task_status_thread = None + + def _run(self) -> None: + transition: Callable + while True: + if self.state_machine.should_stop_mission(): + transition = self.state_machine.stop # type: ignore + break + + if not self.task_status_thread: + self._run_get_status_thread( + status_function=self.state_machine.robot.task_status, + function_argument=self.state_machine.current_task.id, + thread_name="State Machine Returning Home Get Task Status", + ) + try: + status: TaskStatus = self.task_status_thread.get_output() + self.request_status_failure_counter = 0 + except ThreadedRequestNotFinishedError: + time.sleep(self.state_machine.sleep_time) + continue + except ( + RobotCommunicationTimeoutException, + RobotCommunicationException, + ) as e: + retry_limit_exceeded: bool = self._check_if_exceeded_retry_limit(e) + if retry_limit_exceeded: + self.logger.error( + f"Returning to home task {self.state_machine.current_task.id[:8]} failed " + f"because: {e.error_description}" + ) + status = TaskStatus.Failed + else: + time.sleep(settings.REQUEST_STATUS_COMMUNICATION_RECONNECT_DELAY) + continue + + except RobotTaskStatusException as e: + self.state_machine.current_task.error_message = ErrorMessage( + error_reason=e.error_reason, error_description=e.error_description + ) + self.logger.error( + f"Return to home task {self.state_machine.current_task.id[:8]} failed " + f"because: {e.error_description}" + ) + status = TaskStatus.Failed + + except RobotException as e: + self._set_error_message(e) + status = TaskStatus.Failed + + self.logger.error( + f"Retrieving the status failed because: {e.error_description}" + ) + + if not isinstance(status, TaskStatus): + self.logger.error( + f"Received an invalid status update {status} when returning home. " + "Only TaskStatus is expected." + ) + break + + if self.state_machine.current_task is None: + self.state_machine.iterate_current_task() + + self.state_machine.current_task.status = status + + if self.state_machine.current_task.is_finished(): + self._report_task_status(self.state_machine.current_task) + self.state_machine.publish_task_status( + task=self.state_machine.current_task + ) + + self.state_machine.iterate_current_task() + if self.state_machine.current_task is None: + transition = self.state_machine.return_home_finished # type: ignore + break + + # Report and update next task + self.state_machine.current_task.update_task_status() + self.state_machine.publish_task_status( + task=self.state_machine.current_task + ) + + self.task_status_thread = None + time.sleep(self.state_machine.sleep_time) + + transition() + + def _run_get_status_thread( + self, status_function: Callable, function_argument: str, thread_name: str + ) -> None: + self.task_status_thread = ThreadedRequest(request_func=status_function) + self.task_status_thread.start_thread(function_argument, name=thread_name) + + def _report_task_status(self, task: Task) -> None: + if task.status == TaskStatus.Failed: + self.logger.warning( + f"Task: {str(task.id)[:8]} was reported as failed by the robot" + ) + elif task.status == TaskStatus.Successful: + self.logger.info( + f"{type(task).__name__} task: {str(task.id)[:8]} completed" + ) + + def _set_error_message(self, e: RobotException) -> None: + error_message: ErrorMessage = ErrorMessage( + error_reason=e.error_reason, error_description=e.error_description + ) + self.state_machine.current_task.error_message = error_message + + def _check_if_exceeded_retry_limit( + self, e: Union[RobotCommunicationTimeoutException, RobotCommunicationException] + ) -> bool: + self.state_machine.current_mission.error_message = ErrorMessage( + error_reason=e.error_reason, error_description=e.error_description + ) + self.task_status_thread = None + self.request_status_failure_counter += 1 + self.logger.warning( + f"Return to home task {self.state_machine.current_task.id} failed #: " + f"{self.request_status_failure_counter} failed because: {e.error_description}" + ) + + if ( + self.request_status_failure_counter + >= self.request_status_failure_counter_limit + ): + self.state_machine.current_task.error_message = ErrorMessage( + error_reason=e.error_reason, + error_description=e.error_description, + ) + self.logger.error( + f"Task will be cancelled after failing to get task status " + f"{self.request_status_failure_counter} times because: " + f"{e.error_description}" + ) + return True + + return False diff --git a/src/isar/state_machine/states_enum.py b/src/isar/state_machine/states_enum.py index a027c69e..d91a2b5c 100644 --- a/src/isar/state_machine/states_enum.py +++ b/src/isar/state_machine/states_enum.py @@ -3,10 +3,13 @@ class States(str, Enum): Off = "off" + Docked = "docked" Idle = "idle" Initiate = "initiate" Initialize = "initialize" Monitor = "monitor" + ReturningHome = "returning_home" + AwaitNextMission = "await_next_mission" Paused = "paused" Stop = "stop" Offline = "offline" diff --git a/src/robot_interface/models/mission/status.py b/src/robot_interface/models/mission/status.py index 4f1e7f11..44190ea0 100644 --- a/src/robot_interface/models/mission/status.py +++ b/src/robot_interface/models/mission/status.py @@ -24,6 +24,7 @@ class TaskStatus(str, Enum): class RobotStatus(Enum): Available = "available" Busy = "busy" + Docked = "docked" Offline = "offline" Blocked = "blocked" BlockedProtectiveStop = "blockedprotectivestop" diff --git a/src/robot_interface/models/mission/task.py b/src/robot_interface/models/mission/task.py index 2cd33691..c8ddb6d5 100644 --- a/src/robot_interface/models/mission/task.py +++ b/src/robot_interface/models/mission/task.py @@ -74,7 +74,6 @@ class ReturnToHome(Task): Task which cases the robot to return home """ - pose: Pose = Field(default=None) type: Literal[TaskTypes.ReturnToHome] = TaskTypes.ReturnToHome diff --git a/tests/isar/mission/test_mission.py b/tests/isar/mission/test_mission.py index 33a112cd..5a4835a8 100644 --- a/tests/isar/mission/test_mission.py +++ b/tests/isar/mission/test_mission.py @@ -35,13 +35,7 @@ robot_pose=robot_pose_2, ) -task_return_to_home = ReturnToHome( - pose=Pose( - position=Position(x=0, y=0, z=0, frame=Frame(name="asset")), - orientation=Orientation(x=0, y=0, z=0, w=1, frame=Frame(name="asset")), - frame=Frame(name="asset"), - ), -) +task_return_to_home = ReturnToHome() expected_mission = Mission( id="1", diff --git a/tests/isar/services/readers/test_mission_reader.py b/tests/isar/services/readers/test_mission_reader.py index 618688da..17b25097 100644 --- a/tests/isar/services/readers/test_mission_reader.py +++ b/tests/isar/services/readers/test_mission_reader.py @@ -30,44 +30,31 @@ def test_get_mission_with_no_tasks(mission_reader: LocalPlanner) -> None: def test_read_mission_from_file(mission_reader: LocalPlanner) -> None: expected_robot_pose_1 = Pose( - position=Position(-2, -2, 0, Frame("asset")), - orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")), - frame=Frame("asset"), - ) - task_1: ReturnToHome = ReturnToHome(pose=expected_robot_pose_1) - - expected_robot_pose_2 = Pose( position=Position(-2, 2, 0, Frame("asset")), orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")), frame=Frame("asset"), ) expected_inspection_target_1 = Position(2, 2, 0, Frame("asset")) - task_2: TakeImage = TakeImage( - target=expected_inspection_target_1, robot_pose=expected_robot_pose_2 + task_1: TakeImage = TakeImage( + target=expected_inspection_target_1, robot_pose=expected_robot_pose_1 ) - expected_robot_pose_3 = Pose( + expected_robot_pose_2 = Pose( position=Position(2, 2, 0, Frame("asset")), orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")), frame=Frame("asset"), ) expected_inspection_target_2 = Position(2, 2, 0, Frame("asset")) - task_3: TakeImage = TakeImage( - target=expected_inspection_target_2, robot_pose=expected_robot_pose_3 + task_2: TakeImage = TakeImage( + target=expected_inspection_target_2, robot_pose=expected_robot_pose_2 ) - expected_robot_pose_4 = Pose( - position=Position(0, 0, 0, Frame("asset")), - orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")), - frame=Frame("asset"), - ) - task_4: ReturnToHome = ReturnToHome(pose=expected_robot_pose_4) + task_3: ReturnToHome = ReturnToHome() expected_tasks: List[TASKS] = [ task_1, task_2, task_3, - task_4, ] mission: Mission = mission_reader.read_mission_from_file( Path("./tests/test_data/test_mission_working.json") @@ -75,7 +62,9 @@ def test_read_mission_from_file(mission_reader: LocalPlanner) -> None: for expected_task, task in zip(expected_tasks, mission.tasks): if isinstance(expected_task, ReturnToHome) and isinstance(task, ReturnToHome): - assert expected_task.pose == task.pose + assert isinstance(expected_task, ReturnToHome) and isinstance( + task, ReturnToHome + ) if isinstance(expected_task, TakeImage) and isinstance(task, TakeImage): assert expected_task.target == task.target assert expected_task.robot_pose == task.robot_pose diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 66d14121..d8b80a73 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -10,6 +10,7 @@ from isar.models.communication.queues.queues import Queues from isar.services.utilities.scheduling_utilities import SchedulingUtilities from isar.state_machine.state_machine import StateMachine, main +from isar.state_machine.states.docked import Docked from isar.state_machine.states.idle import Idle from isar.state_machine.states_enum import States from isar.storage.storage_interface import StorageInterface @@ -25,6 +26,8 @@ from tests.mocks.pose import MockPose from tests.mocks.robot_interface import ( MockRobot, + MockRobotIdleToDockedToIdleTest, + MockRobotOfflineToDockedTest, MockRobotIdleToOfflineToIdleTest, MockRobotIdleToBlockedProtectiveStopToIdleTest, ) @@ -85,18 +88,18 @@ def test_reset_state_machine(state_machine) -> None: def test_state_machine_transitions_when_running_full_mission( injector, state_machine_thread ) -> None: - state_machine_thread.state_machine.run_mission_by_task = False + state_machine_thread.state_machine.await_next_mission_state.return_home_delay = 0.1 state_machine_thread.start() task_1: Task = TakeImage( target=MockPose.default_pose().position, robot_pose=MockPose.default_pose() ) - task_2: Task = ReturnToHome(pose=MockPose.default_pose()) + task_2: Task = ReturnToHome() mission: Mission = Mission(name="Dummy misson", tasks=[task_1, task_2]) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission) - time.sleep(0.21) # Slightly more than the StateMachine sleep time + time.sleep(1) # Allow enough time to run mission and return home assert state_machine_thread.state_machine.transitions_list == deque( [ @@ -104,6 +107,10 @@ def test_state_machine_transitions_when_running_full_mission( States.Initialize, States.Initiate, States.Monitor, + States.AwaitNextMission, + States.Initialize, + States.Initiate, + States.ReturningHome, States.Idle, ] ) @@ -115,16 +122,17 @@ def test_state_machine_failed_dependency( task_1: Task = TakeImage( target=MockPose.default_pose().position, robot_pose=MockPose.default_pose() ) - task_2: Task = ReturnToHome(pose=MockPose.default_pose()) + task_2: Task = ReturnToHome() mission: Mission = Mission(name="Dummy misson", tasks=[task_1, task_2]) mocker.patch.object(MockRobot, "task_status", return_value=TaskStatus.Failed) + state_machine_thread.state_machine.await_next_mission_state.return_home_delay = 0.1 state_machine_thread.start() scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission) - time.sleep(0.21) # Allow the state machine to transition through the mission + time.sleep(1) # Allow enough time to run mission and return home assert state_machine_thread.state_machine.transitions_list == deque( [ @@ -132,6 +140,10 @@ def test_state_machine_failed_dependency( States.Initialize, States.Initiate, States.Monitor, + States.AwaitNextMission, + States.Initialize, + States.Initiate, + States.ReturningHome, States.Idle, ] ) @@ -140,6 +152,7 @@ def test_state_machine_failed_dependency( def test_state_machine_with_successful_collection( injector, state_machine_thread, uploader_thread ) -> None: + state_machine_thread.state_machine.await_next_mission_state.return_home_delay = 0.1 state_machine_thread.start() storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] @@ -148,7 +161,7 @@ def test_state_machine_with_successful_collection( scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission) - time.sleep(0.11) # Slightly more than the StateMachine sleep time + time.sleep(1) # Allow enough time to run mission and return home expected_stored_items = 1 assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore @@ -158,6 +171,10 @@ def test_state_machine_with_successful_collection( States.Initialize, States.Initiate, States.Monitor, + States.AwaitNextMission, + States.Initialize, + States.Initiate, + States.ReturningHome, States.Idle, ] ) @@ -170,13 +187,14 @@ def test_state_machine_with_unsuccessful_collection( mocker.patch.object(MockRobot, "get_inspection", return_value=[]) + state_machine_thread.state_machine.await_next_mission_state.return_home_delay = 0.1 state_machine_thread.start() mission: Mission = Mission(name="Dummy misson", tasks=[MockTask.take_image()]) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission) - time.sleep(0.11) # Slightly more than the StateMachine sleep time + time.sleep(1) # Allow enough time to run mission and return home expected_stored_items = 0 assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore @@ -187,6 +205,10 @@ def test_state_machine_with_unsuccessful_collection( States.Initialize, States.Initiate, States.Monitor, + States.AwaitNextMission, + States.Initialize, + States.Initiate, + States.ReturningHome, States.Idle, ] ) @@ -257,7 +279,6 @@ def test_state_machine_with_unsuccessful_mission_stop( def test_state_machine_idle_to_offline_to_idle(mocker, state_machine_thread) -> None: - # Robot status check happens every 5 seconds by default, so we mock the behavior # to poll for status imediately mocker.patch.object(Idle, "_is_ready_to_poll_for_status", return_value=True) @@ -274,7 +295,6 @@ def test_state_machine_idle_to_offline_to_idle(mocker, state_machine_thread) -> def test_state_machine_idle_to_blocked_protective_stop_to_idle( mocker, state_machine_thread ) -> None: - # Robot status check happens every 5 seconds by default, so we mock the behavior # to poll for status imediately mocker.patch.object(Idle, "_is_ready_to_poll_for_status", return_value=True) @@ -290,6 +310,36 @@ def test_state_machine_idle_to_blocked_protective_stop_to_idle( ) +def test_state_machine_idle_to_docked_to_idle(mocker, state_machine_thread) -> None: + # Robot status check happens every 5 seconds by default, so we mock the behavior + # to poll for status imediately + mocker.patch.object(Idle, "_is_ready_to_poll_for_status", return_value=True) + mocker.patch.object(Docked, "_is_ready_to_poll_for_status", return_value=True) + + state_machine_thread.state_machine.robot = MockRobotIdleToDockedToIdleTest() + state_machine_thread.start() + time.sleep(0.11) # Slightly more than the StateMachine sleep time + + assert state_machine_thread.state_machine.transitions_list == deque( + [States.Idle, States.Docked, States.Idle] + ) + + +def test_state_machine_idle_to_offline_to_docked(mocker, state_machine_thread) -> None: + # Robot status check happens every 5 seconds by default, so we mock the behavior + # to poll for status imediately + mocker.patch.object(Idle, "_is_ready_to_poll_for_status", return_value=True) + mocker.patch.object(Docked, "_is_ready_to_poll_for_status", return_value=True) + + state_machine_thread.state_machine.robot = MockRobotOfflineToDockedTest() + state_machine_thread.start() + time.sleep(0.11) # Slightly more than the StateMachine sleep time + + assert state_machine_thread.state_machine.transitions_list == deque( + [States.Idle, States.Offline, States.Idle, States.Docked] + ) + + def _mock_robot_exception_with_message() -> RobotException: raise RobotException( error_reason=ErrorReason.RobotUnknownErrorException, diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index 0b2ebed6..af107342 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -119,3 +119,27 @@ def robot_status(self) -> RobotStatus: return RobotStatus.BlockedProtectiveStop return RobotStatus.Available + + +class MockRobotIdleToDockedToIdleTest(MockRobot): + def __init__(self): + self.first = True + + def robot_status(self) -> RobotStatus: + if self.first: + self.first = False + return RobotStatus.Docked + + return RobotStatus.Available + + +class MockRobotOfflineToDockedTest(MockRobot): + def __init__(self): + self.first = True + + def robot_status(self) -> RobotStatus: + if self.first: + self.first = False + return RobotStatus.Offline + + return RobotStatus.Docked diff --git a/tests/mocks/task.py b/tests/mocks/task.py index cb083b26..da32d076 100644 --- a/tests/mocks/task.py +++ b/tests/mocks/task.py @@ -1,13 +1,12 @@ from alitra import Frame, Orientation, Pose, Position from robot_interface.models.mission.task import ReturnToHome, TakeImage -from tests.mocks.pose import MockPose class MockTask: @staticmethod def return_home() -> ReturnToHome: - return ReturnToHome(pose=MockPose.default_pose()) + return ReturnToHome() @staticmethod def take_image() -> TakeImage: diff --git a/tests/test_data/test_mission_working.json b/tests/test_data/test_mission_working.json index 4699acbe..0c780156 100644 --- a/tests/test_data/test_mission_working.json +++ b/tests/test_data/test_mission_working.json @@ -2,25 +2,6 @@ "id": "1", "name": "Well defined mission", "tasks": [ - { - "type": "return_to_home", - "pose": { - "position": { - "x": -2, - "y": -2, - "z": 0, - "frame": {"name": "asset"} - }, - "orientation": { - "x": 0, - "y": 0, - "z": 0.4794255, - "w": 0.8775826, - "frame": {"name": "asset"} - }, - "frame": {"name": "asset"} - } - }, { "type": "take_image", "robot_pose": { @@ -72,23 +53,7 @@ } }, { - "type": "return_to_home", - "pose": { - "position": { - "x": 0, - "y": 0, - "z": 0, - "frame": {"name": "asset"} - }, - "orientation": { - "x": 0, - "y": 0, - "z": 0.4794255, - "w": 0.8775826, - "frame": {"name": "asset"} - }, - "frame": {"name": "asset"} - } + "type": "return_to_home" } ] }