Merge pull request #1043 from tue-robotics/feat/ignore_z

Implementing ignore_z of ED simple query

challenge_manipulation/src/empty_shelf_designator.py

@@ -133,7 +133,7 @@ def determine_points_of_interest_with_area(self, e, area):
         :return:
         """
         # Just to be sure, copy e
-        e = self.robot.ed.get_entity(id=e.id, parse=True)
+        e = self.robot.ed.get_entity(id=e.id)
 
         # We want to give it a convex hull using the designated area
         if area in e.volumes:

challenge_manipulation/src/manipulation.py

@@ -257,7 +257,7 @@ def execute(self, userdata=None):
         ''' Get cabinet entity '''
         rospy.sleep(rospy.Duration(0.25))  # Sleep for a while to make
         # sure that the robot is actually in ED
-        cabinet_entity = self.robot.ed.get_entity(id=CABINET, parse=True)
+        cabinet_entity = self.robot.ed.get_entity(id=CABINET)
 
         ''' Get the pose of all shelves '''
         shelves = []
@@ -278,7 +278,7 @@ def execute(self, userdata=None):
             center_vector = vector_stamped.vector
 
             # ''' Get entities '''
-            # shelf_entity = self.robot.ed.get_entity(id=shelf, parse=False)
+            # shelf_entity = self.robot.ed.get_entity(id=shelf)
 
             # if shelf_entity:
 
@@ -313,7 +313,7 @@ def execute(self, userdata=None):
             segmented_entities = self.robot.ed.update_kinect("{} {}".format(shelf['name'], cabinet_entity.id))
 
             for id_ in segmented_entities.new_ids:
-                entity = self.robot.ed.get_entity(id=id_, parse=False)  # In simulation, the entity type is not yet updated...
+                entity = self.robot.ed.get_entity(id=id_)  # In simulation, the entity type is not yet updated...
                 SEGMENTED_ENTITIES.append((entity, id_))
 
             entity_types_and_probs = self.robot.ed.classify(ids=segmented_entities.new_ids, types=OBJECT_TYPES)
@@ -328,7 +328,7 @@ def execute(self, userdata=None):
             # DETECTED_OBJECTS_WITH_PROBS = [(e.id, e.type) for e in entity_types_and_probs]
             # DETECTED_OBJECTS_WITH_PROBS = [(e.id, e.type) for e in sorted(entity_types_and_probs, key=lambda o: o[1], reverse=True)]
             for e in entity_types_and_probs:
-                entity = self.robot.ed.get_entity(id=e.id, parse=False)  # In simulation, the entity type is not yet updated...
+                entity = self.robot.ed.get_entity(id=e.id)  # In simulation, the entity type is not yet updated...
                 DETECTED_OBJECTS_WITH_PROBS.append((entity, e.probability))
 
             # print "Detected obs with props 1: {0}".format(DETECTED_OBJECTS_WITH_PROBS)
@@ -367,7 +367,7 @@ def __init__(self, robot):
 
     def execute(self, userdata=None):
 
-        entities = self.robot.ed.get_entities(parse=False)
+        entities = self.robot.ed.get_entities()
 
         for e in entities:
             if not e.is_a("furniture") and e.id != '_root':

challenge_storing_groceries/src/challenge_storing_groceries/empty_shelf_designator.py

@@ -105,7 +105,7 @@ def determine_points_of_interest_with_area(self, e, area):
         :return:
         """
         # Just to be sure, copy e
-        e = self.robot.ed.get_entity(id=e.id, parse=True)
+        e = self.robot.ed.get_entity(id=e.id)
 
         # We want to give it a convex hull using the designated area
 

challenge_storing_groceries/src/challenge_storing_groceries/inspect_shelves.py

@@ -26,7 +26,7 @@ def execute(self, userdata=None):
         # Get cabinet entity
         # Sleep for a while to make sure that the robot is actually in ED
         rospy.sleep(rospy.Duration(0.25))
-        cabinet_entity = self.robot.ed.get_entity(id=self.cabinet.id_, parse=True)
+        cabinet_entity = self.robot.ed.get_entity(id=self.cabinet.id_)
 
         # Get the pose of all shelves
         shelves = []
@@ -68,7 +68,7 @@ def execute(self, userdata=None):
 
             for id_ in segmented_entities.new_ids:
                 # In simulation, the entity type is not yet updated...
-                entity = self.robot.ed.get_entity(id=id_, parse=False)
+                entity = self.robot.ed.get_entity(id=id_)
                 config.SEGMENTED_ENTITIES.append((entity, id_))
             # print "Config.SEGMENTED_ENTITIES: {}".format(config.SEGMENTED_ENTITIES)
 
@@ -85,7 +85,7 @@ def execute(self, userdata=None):
 
             for e in entity_types_and_probs:
                 # In simulation, the entity type is not yet updated...
-                entity = self.robot.ed.get_entity(id=e.id, parse=False)
+                entity = self.robot.ed.get_entity(id=e.id)
                 config.DETECTED_OBJECTS_WITH_PROBS.append((entity, e.probability))
 
             config.DETECTED_OBJECTS_WITH_PROBS = sorted(config.DETECTED_OBJECTS_WITH_PROBS, key=lambda o: o[1],

challenge_where_is_this/src/challenge_where_is_this/inform_machine.py

@@ -63,7 +63,7 @@ def _resolve(self):
         if entity_id is None:
             return None
 
-        entities = self._robot.ed.get_entities(id=entity_id, parse=self.parse)
+        entities = self._robot.ed.get_entities(id=entity_id)
         if entities:
             return entities[0]
         else:

robot_skills/src/robot_skills/mockbot.py

@@ -244,7 +244,7 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs):
         self._static_entities['cabinet'] = self.generate_random_entity(id='cabinet')
 
         self.get_closest_entity = lambda *args, **kwargs: random.choice(self._entities.values())
-        self.get_entity = lambda id=None, parse=True: self._entities[id]
+        self.get_entity = lambda id=None: self._entities[id]
         self.reset = lambda *args, **kwargs: self._dynamic_entities.clear()
         self.navigation = mock.MagicMock()
         self.navigation.get_position_constraint = mock.MagicMock()
@@ -256,15 +256,18 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs):
 
         self._person_names = []
 
-    def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", parse=True):
+    def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", ignore_z=False):
 
         center_point_in_map = center_point.projectToFrame("/map", self.tf_listener)
 
         entities = self._entities.values()
         if type:
             entities = [e for e in entities if e.is_a(type)]
         if radius:
-            entities = [e for e in entities if e.distance_to_2d(center_point_in_map.vector) <= radius]
+            if ignore_z:
+                entities = [e for e in entities if e.distance_to_2d(center_point_in_map.vector) <= radius]
+            else:
+                entities = [e for e in entities if e.distance_to_3d(center_point_in_map.vector) <= radius]
         if id:
             entities = [e for e in entities if e.id == id]
 

robot_skills/src/robot_skills/world_model_ed.py

@@ -93,19 +93,28 @@ def wait_for_connections(self, timeout, log_failing_connections=True):
     #                                             QUERYING
     # ----------------------------------------------------------------------------------------------------
 
-    def get_entities(self, type="", center_point=VectorStamped(), radius=float('inf'), id="", parse=True):
+    def get_entities(self, type="", center_point=VectorStamped(), radius=float('inf'), id="", ignore_z=False):
+        """
+        Get entities via Simple Query interface
+
+        :param type: Type of entity
+        :param center_point: Point from which radius is measured
+        :param radius: Distance between center_point and entity
+        :param id: ID of entity
+        :param ignore_z: Consider only the distance in the X,Y plane for the radius from center_point
+        """
         self._publish_marker(center_point, radius)
 
         center_point_in_map = center_point.projectToFrame("/map", self.tf_listener)
         query = SimpleQueryRequest(id=id, type=type, center_point=kdl_vector_to_point_msg(center_point_in_map.vector),
-                                   radius=radius)
+                                   radius=radius, ignore_z=ignore_z)
 
         try:
             entity_infos = self._ed_simple_query_srv(query).entities
             entities = map(from_entity_info, entity_infos)
         except Exception as e:
-            rospy.logerr("ERROR: robot.ed.get_entities(id=%s, type=%s, center_point=%s, radius=%s)" % (
-                id, type, str(center_point), str(radius)))
+            rospy.logerr("ERROR: robot.ed.get_entities(id={}, type={}, center_point={}, radius={}, ignore_z={})".format(
+                id, type, str(center_point), str(radius), ignore_z))
             rospy.logerr("L____> [%s]" % e)
             return []
 
@@ -150,27 +159,8 @@ def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius
         :param ignore_z: Consider only the distance in the X,Y plane for the radius from center_point criterium.
         :return: list of Entity
         """
-        if ignore_z:
-            # ED does not allow to ignore Z through its interface, so it has to be solved here.
-            #
-            # If we want to ignore the z of entities when checking their distance from center_point,
-            # the simplest thing to do is to set the Z of the center_point to the same value,
-            # so that delta Z is always 0.
-            # But then we first need to know Z (preferably without an additonal parameter or something robot specific)
-            # So, we query ED for other laser entities. These are *assumed* to all have the same Z,
-            # so we can just take one and use its Z and substitute that into the center_point.
-            # To 'just take one', we take entities from a larger range.
-            entities_for_height = self.get_entities(type="", center_point=center_point, radius=sqrt(2**2 + radius**2))
-            if entities_for_height:
-                override_z = entities_for_height[0].frame.extractVectorStamped().vector.z()
-                rospy.logwarn("ignoring Z, so overriding z to be equal to laser height: {}".format(override_z))
-                center_point = VectorStamped(center_point.vector.x(),
-                                             center_point.vector.y(),
-                                             override_z)
-            else:
-                return None
 
-        entities = self.get_entities(type="", center_point=center_point, radius=radius)
+        entities = self.get_entities(type="", center_point=center_point, radius=radius, ignore_z=ignore_z)
 
         # HACK
         entities = [e for e in entities if e.shape and e.type == "" and e.id.endswith("-laser")]
@@ -189,10 +179,10 @@ def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius
 
         return entities[0]
 
-    def get_entity(self, id, parse=True):
-        entities = self.get_entities(id=id, parse=parse)
+    def get_entity(self, id):
+        entities = self.get_entities(id=id)
         if len(entities) == 0:
-            rospy.logwarn("Could not get_entity(id='{}')".format(id))
+            rospy.logerr("Could not get_entity(id='{}')".format(id))
             return None
 
         return entities[0]
@@ -438,7 +428,7 @@ def mesh_entity_in_view(self, id, type=""):
 
     def get_full_id(self, short_id):
         """Get an entity's full ID based on the first characters of its ID like you can do with git hashes"""
-        all_entities = self.get_entities(parse=False)
+        all_entities = self.get_entities()
         matches = filter(lambda fill_id: fill_id.startswith(short_id), [entity.id for entity in all_entities])
         return matches
 

robot_smach_states/examples/navigation/example_random_nav2.py

@@ -51,7 +51,7 @@ def getRandomGoal(self):
             return self.entity_id
 
         # Get all entities
-        entities = self._robot.ed.get_entities(type="", parse=False)
+        entities = self._robot.ed.get_entities(type="")
 
         # Temp: only pick close targets
         #if entities:

robot_smach_states/src/robot_smach_states/manipulation/grab.py

@@ -259,7 +259,7 @@ def associate(self, original_entity):
         :return: associated entity
         """
         # Get all entities
-        entities = self.robot.ed.get_entities(parse=False)
+        entities = self.robot.ed.get_entities()
 
         # Remove all entities with a shape. These are probably not the ones we want to grasp
         for e in entities:

robot_smach_states/src/robot_smach_states/util/designators/ed_designators.py

@@ -27,7 +27,7 @@ class EdEntityCollectionDesignator(Designator):
     >>> check_resolve_type(entities, [Entity]) #This is more a test for check_resolve_type to be honest :-/
     """
 
-    def __init__(self, robot, type="", center_point=None, radius=float('inf'), id="", parse=True, criteriafuncs=None,
+    def __init__(self, robot, type="", center_point=None, radius=float('inf'), id="", criteriafuncs=None,
                  type_designator=None, center_point_designator=None, id_designator=None, debug=False, name=None):
         """Designates a collection of entities of some type, within a radius of some center_point, with some id,
         that match some given criteria functions.
@@ -36,7 +36,6 @@ def __init__(self, robot, type="", center_point=None, radius=float('inf'), id=""
         @param center_point combined with radius: a sphere to search an entity in
         @param radius combined with center_point: a sphere to search an entity in
         @param id the ID of the object to get info about
-        @param parse whether to parse the data string associated with the object model or entity
         @param type_designator same as type but dynamically resolved trhough a designator. Mutually exclusive with type
         @param center_point_designator same as center_point but dynamically resolved through a designator.
                 Mutually exclusive with center_point
@@ -56,7 +55,6 @@ def __init__(self, robot, type="", center_point=None, radius=float('inf'), id=""
         self.center_point = center_point
         self.radius = radius
         self.id = id
-        self.parse = parse
         self.criteriafuncs = criteriafuncs or []
 
         if type_designator:
@@ -79,7 +77,7 @@ def _resolve(self):
         _id = self.id_designator.resolve() if self.id_designator else self.id
         _criteria = self.criteriafuncs
 
-        entities = self.ed.get_entities(_type, _center_point, self.radius, _id, self.parse)
+        entities = self.ed.get_entities(_type, _center_point, self.radius, _id)
         if self.debug:
             import ipdb;
             ipdb.set_trace()
@@ -102,7 +100,7 @@ class EdEntityDesignator(Designator):
     Resolves to an entity from an Ed query
     """
 
-    def __init__(self, robot, type="", center_point=None, radius=float('inf'), id="", parse=True, criteriafuncs=None,
+    def __init__(self, robot, type="", center_point=None, radius=float('inf'), id="", criteriafuncs=None,
                  weight_function=None, type_designator=None, center_point_designator=None, id_designator=None,
                  debug=False, name=None):
         """Designates an entity of some type, within a radius of some center_point, with some id,
@@ -112,7 +110,6 @@ def __init__(self, robot, type="", center_point=None, radius=float('inf'), id=""
         @param center_point combined with radius: a sphere to search an entity in
         @param radius combined with center_point: a sphere to search an entity in
         @param id the ID of the object to get info about
-        @param parse whether to parse the data string associated with the object model or entity
         @param criteriafuncs a list of functions that take an entity and return a bool (True if criterium met)
         @param weight_function returns a weight for each entity, the one with the lowest weight will be selected
         (could be a distance calculation)
@@ -135,7 +132,6 @@ def __init__(self, robot, type="", center_point=None, radius=float('inf'), id=""
         self.center_point = center_point
         self.radius = radius
         self.id = id
-        self.parse = parse
         self.criteriafuncs = criteriafuncs or []
         self.weight_function = weight_function or (lambda entity: 0)
 
@@ -178,7 +174,7 @@ def _resolve(self):
             typechecker = lambda entity: entity.type in _type
             _criteria += [typechecker]
 
-        entities = self.ed.get_entities(_type, _center_point, self.radius, _id, self.parse)
+        entities = self.ed.get_entities(_type, _center_point, self.radius, _id)
         if entities:
             for criterium in _criteria:
                 criterium_code = inspect.getsource(criterium)
@@ -211,22 +207,20 @@ def _resolve(self):
 
 
 class EntityByIdDesignator(Designator):
-    def __init__(self, robot, id, parse=True, name=None):
+    def __init__(self, robot, id, name=None):
         """
         Designate an entity by its ID. Resolves to the entity with that ID
 
         :param robot: Robot who's worldmodel to use
         :param id: ID of the entity. If no such ID, resolves to None
-        :param parse: Whether to parse the Entity's data-field
         :param name: Name of the designator for introspection purposes
         """
         super(EntityByIdDesignator, self).__init__(resolve_type=Entity, name=name)
         self.ed = robot.ed
         self.id_ = id
-        self.parse = parse
 
     def _resolve(self):
-        entities = self.ed.get_entities(id=self.id_, parse=self.parse)
+        entities = self.ed.get_entities(id=self.id_)
         if entities:
             return entities[0]
         else:
@@ -258,7 +252,7 @@ def _resolve(self):
             return None
         rospy.loginfo("first_answer is:", str(first_answer))
 
-        entities = self.ed.get_entities(id=str(first_answer), parse=True)
+        entities = self.ed.get_entities(id=str(first_answer))
         if entities:
             return entities[0]
         else: