Implement step status

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()
 

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_step_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]

src/isar_exr/robotinterface.py

@@ -19,12 +19,14 @@
     align_maps,
 )
 from isar_exr.models.exceptions import NoMissionRunningException
+from isar_exr.models.step_status import ExrStepStatus
 from robot_interface.models.exceptions.robot_exceptions import (
     RobotCommunicationException,
     RobotInfeasibleStepException,
     RobotInitializeException,
     RobotMissionNotSupportedException,
     RobotMissionStatusException,
+    RobotStepStatusException,
 )
 from robot_interface.models.initialize import InitializeParams
 from robot_interface.models.inspection.inspection import Inspection
@@ -86,6 +88,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 +185,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 +214,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 +242,53 @@ 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)
+            )
+            step_status: StepStatus = ExrStepStatus(mission_status).to_step_status()
+        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 step_status == MissionStatus.Cancelled:
+            logging.info("Mission was cancelled")
+
+        return step_status
 
     def stop(self) -> None:
         try:
@@ -460,12 +515,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 +534,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 +548,4 @@ def _add_dock_robot_task_to_mission(
             task_id=dock_task_id,
             mission_definition_id=mission_definition_id,
         )
+        return dock_task_id