diff --git a/src/isar_robot/config/settings.py b/src/isar_robot/config/settings.py index 0c068e6e..0ba9791e 100644 --- a/src/isar_robot/config/settings.py +++ b/src/isar_robot/config/settings.py @@ -19,6 +19,8 @@ def __init__(self) -> None: SHOULD_FAIL_NORMAL_TASK: bool = Field(default=False) SHOULD_FAIL_RETURN_TO_HOME_TASK: bool = Field(default=False) SHOULD_FAIL_RETURN_TO_HOME_MISSION: bool = Field(default=False) + SHOULD_FAIL_TO_STOP_NORMAL_MISSION: bool = Field(default=False) + SHOULD_FAIL_TO_STOP_RETURN_TO_HOME_MISSION: bool = Field(default=False) SHOULD_HAVE_RANDOM_BATTERY_LEVEL: bool = Field(default=False) ROBOT_POSE_PUBLISH_INTERVAL: float = Field(default=5) ROBOT_BATTERY_PUBLISH_INTERVAL: float = Field(default=2) diff --git a/src/isar_robot/robotinterface.py b/src/isar_robot/robotinterface.py index ccc4d3f5..736e29dd 100644 --- a/src/isar_robot/robotinterface.py +++ b/src/isar_robot/robotinterface.py @@ -6,6 +6,7 @@ from threading import Thread from typing import Callable, List, Optional +from robot_interface.models.exceptions.robot_exceptions import RobotActionException from robot_interface.models.inspection.inspection import Inspection from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.status import RobotStatus, TaskStatus @@ -77,6 +78,17 @@ def task_status(self, task_id: str) -> TaskStatus: return TaskStatus.Successful def stop(self) -> None: + if ( + isinstance(self.current_task, ReturnToHome) + and settings.SHOULD_FAIL_TO_STOP_RETURN_TO_HOME_MISSION + ): + raise RobotActionException("Failed to stop return home missios") + if ( + not isinstance(self.current_task, ReturnToHome) + and self.current_task is not None + and settings.SHOULD_FAIL_TO_STOP_NORMAL_MISSION + ): + raise RobotActionException("Failed to stop missios") return def get_inspection(self, task: InspectionTask) -> Inspection: