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

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