From 29ba39e17ea55aee5a033997dc18f00fa95b1390 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Chirico=20Indreb=C3=B8?= Date: Mon, 29 Jan 2024 14:42:01 +0100 Subject: [PATCH] Ignore localise and return to home --- src/isar_exr/robotinterface.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/isar_exr/robotinterface.py b/src/isar_exr/robotinterface.py index fac7ce3..ec45e58 100644 --- a/src/isar_exr/robotinterface.py +++ b/src/isar_exr/robotinterface.py @@ -30,6 +30,7 @@ from robot_interface.models.mission.status import MissionStatus, RobotStatus, StepStatus from robot_interface.models.mission.step import ( DriveToPose, + Localize, InspectionStep, Step, TakeImage, @@ -92,11 +93,26 @@ def initiate_mission(self, mission: Mission) -> None: if curent_stage_id is not None: self.api.discard_stage(stage_id=curent_stage_id) stage_id: str = self.api.create_stage(site_id=settings.ROBOT_EXR_SITE_ID) + updating_site = False poi_ids: List[str] = [] + is_possible_return_to_home_mission = False + steps_n = 0 for task in mission.tasks: for step in task.steps: + steps_n += 1 + if isinstance(step, Localize): + steps_n -= 1 if isinstance(step, DriveToPose): + if ( + step.pose.position.x == 0.0 + and step.pose.position.y == 0.0 + and step.pose.position.z == 0.0 + and step.pose.orientation.x == 0.0 + and step.pose.orientation.y == 0.0 + and step.pose.orientation.z == 0.0 + ): + is_possible_return_to_home_mission = True robot_pose: Pose = step.pose if isinstance(step, InspectionStep): existing_poi_id = self.api.get_point_of_interest_by_customer_tag( @@ -114,6 +130,9 @@ def initiate_mission(self, mission: Mission) -> None: else: poi_ids.append(existing_poi_id) + if steps_n == 0 or (steps_n == 1 and is_possible_return_to_home_mission): + return + if updating_site: # We should only do the following if we changed the site snapshot_id: str = self.api.commit_site_to_snapshot(stage_id=stage_id)