(FIX) separate operator case so we do not need to resolve entity (#109)

separate operator case so we do not need to resolve entity
tue-robotics · Jan 3, 2022 · d5a8aac · d5a8aac
2 parents 7473e93 + 8abbade
commit d5a8aac
Showing 1 changed file with 20 additions and 9 deletions.
diff --git a/action_server/src/action_server/actions/navigate_to.py b/action_server/src/action_server/actions/navigate_to.py
@@ -51,12 +51,6 @@ def _parse_context(context_dict):
     def _configure(self, robot, config):
         self._robot = robot
 
-        # If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
-        # navigate to.
-        # TODO: learn to recognize the operator so that you know you found him later on
-        self._robot.ed.update_entity(uuid="operator", frame_stamped=self._robot.base.get_location(),
-                                     etype="waypoint")
-
         semantics = self._parse_semantics(config.semantics)
         context = self._parse_context(config.context)
 
@@ -94,11 +88,26 @@ def _configure(self, robot, config):
                 self._config_result.required_context['source-location'] = config.semantics['source-location']
             return
         # Now we can assume we know the navigation goal entity!
+        # operator
+        if semantics.target_location.id == 'operator':
+            # If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
+            # navigate to.
+            # TODO: learn to recognize the operator so that you know you found him later on
+            self._robot.ed.update_entity(uuid="operator", frame_stamped=self._robot.base.get_location(),
+                                         etype="waypoint")
+            entity_designator = EntityByIdDesignator(self._robot, uuid=semantics.target_location.id)
+            self._navigation_state_machine = NavigateToWaypoint(self._robot,
+                                                                waypoint_designator=entity_designator,
+                                                                radius=0.1)
+            rospy.loginfo("Navigation set up for a waypoint")
 
-        if semantics.target_location.id and \
-            (semantics.target_location.type != 'person' or semantics.target_location.id == 'operator'):
+        # known room or object
+        elif semantics.target_location.id and semantics.target_location.type != 'person':
             entity_designator = EntityByIdDesignator(self._robot, uuid=semantics.target_location.id)
-            e = entity_designator.resolve()  # TODO: nasty assumption that we can resolve this entity here?!
+            e = entity_designator.resolve()
+
+            if e is None:
+                return  # self._config_result.succeeded = False
 
             if e.is_a("waypoint"):
                 self._navigation_state_machine = NavigateToWaypoint(self._robot,
@@ -119,12 +128,14 @@ def _configure(self, robot, config):
                                                                     entity_designator_area_name_map={
                                                                         entity_designator: area},
                                                                     entity_lookat_designator=entity_designator)
+        # person from context
         elif semantics.target_location.type == 'person' and context.object:
             entity_designator = context.object.designator
             self._navigation_state_machine = NavigateToWaypoint(self._robot,
                                                                 waypoint_designator=entity_designator,
                                                                 radius=0.7)
             rospy.loginfo("Navigation set up for a waypoint")
+        # entity from context
         else:
             entity_designator = context.object.designator
             self._navigation_state_machine = NavigateToSymbolic(self._robot,