diff --git a/src/isar_exr/api/energy_robotics_api.py b/src/isar_exr/api/energy_robotics_api.py index b49341c..fcb20af 100644 --- a/src/isar_exr/api/energy_robotics_api.py +++ b/src/isar_exr/api/energy_robotics_api.py @@ -25,7 +25,7 @@ ) from isar_exr.config.settings import settings from isar_exr.models.exceptions import NoMissionRunningException -from isar_exr.models.step_status import ExrMissionStatus +from isar_exr.models.step_status import ExrMissionStatus, ExrStepStatus def to_dict(obj): @@ -74,6 +74,49 @@ def get_mission_status(self, exr_robot_id: str) -> MissionStatus: ) return step_status.to_mission_status() + def get_mission_status_and_current_task( + self, exr_robot_id: str + ) -> tuple[MissionStatus, str]: + variable_definitions_graphql: DSLVariableDefinitions = DSLVariableDefinitions() + + current_mission_execution_query: DSLQuery = DSLQuery( + self.schema.Query.currentMissionExecution.args( + robotID=variable_definitions_graphql.robotID + ).select( + self.schema.MissionExecutionType.status, + self.schema.MissionExecutionType.currentExecutedTaskId, + ) + ) + + current_mission_execution_query.variable_definitions = ( + variable_definitions_graphql + ) + + params: dict = {"robotID": exr_robot_id} + + if not self.is_mission_running(exr_robot_id): + raise NoMissionRunningException( + f"Cannot get current EXR task - No EXR mission is running for robot " + f"with id {exr_robot_id}" + ) + + response_dict: dict[str, Any] = self.client.query( + dsl_gql(current_mission_execution_query), params + ) + + if response_dict["currentMissionExecution"] is None: + raise NoMissionRunningException( + f"Cannot get current EXR task - No EXR mission is running for robot " + f"with id {exr_robot_id}" + ) + + step_status = ExrStepStatus(response_dict["currentMissionExecution"]["status"]) + + return ( + step_status, + response_dict["currentMissionExecution"]["currentExecutedTaskId"], + ) + def is_mission_running(self, exr_robot_id: str) -> bool: variable_definitions_graphql: DSLVariableDefinitions = DSLVariableDefinitions() diff --git a/src/isar_exr/models/step_status.py b/src/isar_exr/models/step_status.py index 3163b0c..7e91410 100644 --- a/src/isar_exr/models/step_status.py +++ b/src/isar_exr/models/step_status.py @@ -1,6 +1,6 @@ from enum import Enum -from robot_interface.models.mission.status import MissionStatus +from robot_interface.models.mission.status import MissionStatus, StepStatus class ExrMissionStatus(str, Enum): @@ -13,6 +13,7 @@ class ExrMissionStatus(str, Enum): InProgress: str = "IN_PROGRESS" Paused: str = "PAUSED" Completed: str = "COMPLETED" + ResetRequested: str = "RESET_REQUESTED" def to_mission_status(self) -> MissionStatus: return { @@ -26,4 +27,32 @@ def to_mission_status(self) -> MissionStatus: ExrMissionStatus.InProgress: MissionStatus.InProgress, ExrMissionStatus.Paused: MissionStatus.Cancelled, ExrMissionStatus.Completed: MissionStatus.Successful, + ExrMissionStatus.ResetRequested: MissionStatus.Cancelled, + }[self] + + +class ExrStepStatus(str, Enum): + StartRequested: str = "START_REQUESTED" + PauseRequested: str = "PAUSE_REQUESTED" + ResumeRequested: str = "RESUME_REQUESTED" + Rejected: str = "REJECTED" + WakingUp: str = "WAKING_UP" + Starting: str = "STARTING" + InProgress: str = "IN_PROGRESS" + Paused: str = "PAUSED" + Completed: str = "COMPLETED" + ResetRequested: str = "RESET_REQUESTED" + + def to_mission_status(self) -> StepStatus: + return { + ExrStepStatus.StartRequested: StepStatus.NotStarted, + ExrStepStatus.PauseRequested: StepStatus.InProgress, + ExrStepStatus.ResumeRequested: StepStatus.Cancelled, + ExrStepStatus.Rejected: StepStatus.Failed, + ExrStepStatus.WakingUp: StepStatus.NotStarted, + ExrStepStatus.Starting: StepStatus.NotStarted, + ExrStepStatus.InProgress: StepStatus.InProgress, + ExrStepStatus.Paused: StepStatus.Cancelled, + ExrStepStatus.Completed: StepStatus.Successful, + ExrStepStatus.ResetRequested: StepStatus.Cancelled, }[self] diff --git a/src/isar_exr/robotinterface.py b/src/isar_exr/robotinterface.py index b857d27..176d27d 100644 --- a/src/isar_exr/robotinterface.py +++ b/src/isar_exr/robotinterface.py @@ -25,6 +25,7 @@ RobotInitializeException, RobotMissionNotSupportedException, RobotMissionStatusException, + RobotStepStatusException, ) from robot_interface.models.initialize import InitializeParams from robot_interface.models.inspection.inspection import Inspection @@ -86,6 +87,8 @@ def __init__(self) -> None: self.transform: Transform = align_maps( map_alignment.map_from, map_alignment.map_to, rot_axes="xyz" ) + self.mission_task_ids: List[List[str]] = [] + self.current_mission_task_index: int = 0 def create_new_stage(self) -> str: current_stage_id = self.api.get_current_site_stage(settings.ROBOT_EXR_SITE_ID) @@ -181,22 +184,27 @@ def create_mission_definition( ) for task in tasks: + step_ids: List[str] = [] for step in task.steps: if isinstance(step, DriveToPose): - self._add_waypoint_task_to_mission( + task_id = self._add_waypoint_task_to_mission( mission_definition_id=mission_definition_id, step=step ) + step_ids.append(task_id) if isinstance(step, InspectionStep): - self._add_point_of_interest_inspection_task_to_mission( + task_id = self._add_point_of_interest_inspection_task_to_mission( task_name=step.id, point_of_interest_id=poi_ids.pop(0), mission_definition_id=mission_definition_id, ) + step_ids.append(task_id) + self.mission_task_ids.append(step_ids) - self._add_dock_robot_task_to_mission( + dock_task_id: str = self._add_dock_robot_task_to_mission( task_name="dock", mission_definition_id=mission_definition_id, ) + self.mission_task_ids.append([dock_task_id]) return mission_definition_id def initiate_mission(self, mission: Mission) -> None: @@ -205,6 +213,8 @@ def initiate_mission(self, mission: Mission) -> None: except RobotMissionNotSupportedException: return + self.mission_task_ids = [] + self.current_mission_task_index = 0 mission_definition_id: str = self.create_mission_definition( mission.id, mission.tasks, poi_ids ) @@ -231,9 +241,51 @@ def initiate_step(self, step: Step) -> None: raise NotImplementedError def step_status(self) -> StepStatus: - # TODO: use currentMissionExecution(robotID) - self.logger.error("An invalid interface function was called") - raise NotImplementedError + try: + (mission_status, current_task_id) = ( + self.api.get_mission_status_and_current_task(settings.ROBOT_EXR_ID) + ) + except NoMissionRunningException: + # This is a temporary solution until we have mission status by mission id + return MissionStatus.Successful + except Exception as e: + message: str = "Could not get status of running mission\n" + self.logger.error(message) + raise RobotMissionStatusException( + error_description=message, + ) + + if current_task_id == "end_mission": + return StepStatus.Successful + + if ( + current_task_id == "start_mission" + or current_task_id == None + or current_task_id == "" + ): + return StepStatus.NotStarted + + try: + step: List[str] = list( + filter(lambda step: current_task_id in step, self.mission_task_ids) + )[0] + task_index: int = self.mission_task_ids.index(step) + except ValueError or IndexError: + value_error_message: str = ( + f"Could not find mission task with ID {current_task_id}\n" + ) + self.logger.error(value_error_message) + raise RobotStepStatusException( + error_description=value_error_message, + ) + + if task_index > self.current_mission_task_index: + self.current_mission_task_index += 1 + return StepStatus.Successful + + if mission_status == MissionStatus.Cancelled: + logging.info("Mission was cancelled") + return mission_status def stop(self) -> None: try: @@ -460,12 +512,12 @@ def _add_waypoint_task_to_mission( task_name=step.id, pose_3D_stamped_input=pose_3d_stamped, ) - add_task_id: str = self.api.add_task_to_mission_definition( + self.api.add_task_to_mission_definition( task_id=waypoint_id, mission_definition_id=mission_definition_id, ) - return add_task_id + return waypoint_id def _add_point_of_interest_inspection_task_to_mission( self, task_name: str, point_of_interest_id: str, mission_definition_id: str @@ -479,6 +531,7 @@ def _add_point_of_interest_inspection_task_to_mission( task_id=poi_task_id, mission_definition_id=mission_definition_id, ) + return poi_task_id def _add_dock_robot_task_to_mission( self, task_name: str, mission_definition_id: str @@ -492,3 +545,4 @@ def _add_dock_robot_task_to_mission( task_id=dock_task_id, mission_definition_id=mission_definition_id, ) + return dock_task_id