Merge pull request #985 from tue-robotics/fix/ed_query_default_radius

added default radius as inf

robot_skills/src/robot_skills/world_model_ed.py

@@ -92,7 +92,7 @@ def wait_for_connections(self, timeout, log_failing_connections=True):
     #                                             QUERYING
     # ----------------------------------------------------------------------------------------------------
 
-    def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", parse=True):
+    def get_entities(self, type="", center_point=VectorStamped(), radius=float('inf'), id="", parse=True):
         self._publish_marker(center_point, radius)
 
         center_point_in_map = center_point.projectToFrame("/map", self.tf_listener)
@@ -110,7 +110,7 @@ def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", p
 
         return entities
 
-    def get_closest_entity(self, type="", center_point=None, radius=0):
+    def get_closest_entity(self, type="", center_point=None, radius=float('inf')):
         if not center_point:
             center_point = VectorStamped(x=0, y=0, z=0, frame_id="/" + self.robot_name + "/base_link")
 
@@ -132,13 +132,13 @@ def get_closest_entity(self, type="", center_point=None, radius=0):
 
         return entities[0]
 
-    def get_closest_room(self, center_point=None, radius=0):
+    def get_closest_room(self, center_point=None, radius=float('inf')):
         if not center_point:
             center_point = VectorStamped(x=0, y=0, z=0, frame_id="/" + self.robot_name + "/base_link")
 
         return self.get_closest_entity(type="room", center_point=center_point, radius=radius)
 
-    def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius=0, ignore_z=False):
+    def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius=float('inf'), ignore_z=False):
         """
         Get the closest entity detected by the laser. The ID's of such entities are postfixed with '-laser'
         For the rest, this works exactly like get_closest_entity
@@ -302,7 +302,7 @@ def lock_entities(self, lock_ids, unlock_ids):
 
     # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-    def get_closest_possible_person_entity(self, center_point=VectorStamped(), radius=0):
+    def get_closest_possible_person_entity(self, center_point=VectorStamped(), radius=float('inf')):
         """ Returns the 'possible_human' entity closest to a certain center point.
 
         :param center_point: (VectorStamped) indicating where the human should be close to

robot_smach_states/examples/manipulation/example_place_designator.py

@@ -16,22 +16,22 @@
 
     robot = get_robot(args.robot)
 
-    furniture_designator = ds.EdEntityDesignator(robot, id="dinner_table")
+    furniture_designator = ds.EntityByIdDesignator(robot, id="dinner_table")
     arm_designator = ds.UnoccupiedArmDesignator(robot, {})
 
     def with_area():
         esd = EmptySpotDesignator(robot=robot,
-                                     place_location_designator=furniture_designator,
-                                     arm_designator=arm_designator,
-                                     name="with_area",
-                                     area="on_top_of")
+                                  place_location_designator=furniture_designator,
+                                  arm_designator=arm_designator,
+                                  name="with_area",
+                                  area="on_top_of")
         print(esd.resolve())
 
     def without_area():
         esd = EmptySpotDesignator(robot=robot,
-                                     place_location_designator=furniture_designator,
-                                     arm_designator=arm_designator,
-                                     name="without_area")
+                                  place_location_designator=furniture_designator,
+                                  arm_designator=arm_designator,
+                                  name="without_area")
         print(esd.resolve())
 
     with_area()

robot_smach_states/src/robot_smach_states/manipulation/place_designator.py

@@ -61,18 +61,18 @@ def _resolve(self):
 
         # points_of_interest = []
         if self._area:
-            vectors_of_interest = self.determine_points_of_interest_with_area(place_location, self._area)
+            vectors_of_interest = self._determine_points_of_interest_with_area(place_location, self._area)
         else:
-            vectors_of_interest = self.determine_points_of_interest(place_frame.frame, z_max=place_location.shape.z_max,
+            vectors_of_interest = self._determine_points_of_interest(place_frame.frame, z_max=place_location.shape.z_max,
                                                                     convex_hull=place_location.shape.convex_hull)
 
         assert all(isinstance(v, FrameStamped) for v in vectors_of_interest)
 
-        open_POIs = filter(self.is_poi_occupied, vectors_of_interest)
+        open_POIs = filter(lambda pose: self._is_poi_unoccupied(pose, place_location), vectors_of_interest)
 
         base_pose = self.robot.base.get_location()
         arm = self.arm_designator.resolve()
-        open_POIs_dist = [(poi, self.distance_to_poi_area_heuristic(poi, base_pose, arm)) for poi in open_POIs]
+        open_POIs_dist = [(poi, self._distance_to_poi_area_heuristic(poi, base_pose, arm)) for poi in open_POIs]
 
         # We don't care about small differences
         open_POIs_dist.sort(key=lambda tup:tup[1])
@@ -81,20 +81,21 @@ def _resolve(self):
         open_POIs_dist.sort(key=lambda tup: tup[0].edge_score, reverse=True) # sorts in place
 
         for poi in open_POIs_dist:
-            if self.distance_to_poi_area(poi[0], arm):
-                selection = self.create_selection_marker(poi[0])
+            if self._distance_to_poi_area(poi[0], arm):
+                selection = self._create_selection_marker(poi[0])
                 self.marker_pub.publish(MarkerArray([selection]))
                 return poi[0]
 
         rospy.logerr("Could not find an empty spot")
         return None
 
-    def is_poi_occupied(self, frame_stamped):
+    def _is_poi_unoccupied(self, frame_stamped, surface_entity):
         entities_at_poi = self.robot.ed.get_entities(center_point=frame_stamped.extractVectorStamped(),
                                                      radius=self._spacing)
+        entities_at_poi = [entity for entity in entities_at_poi if entity.id != surface_entity.id]
         return not any(entities_at_poi)
 
-    def distance_to_poi_area_heuristic(self, frame_stamped, base_pose, arm):
+    def _distance_to_poi_area_heuristic(self, frame_stamped, base_pose, arm):
         """
         :return: direct distance between a point and and the place offset of the arm
         :rtype: double [meters]
@@ -111,7 +112,7 @@ def distance_to_poi_area_heuristic(self, frame_stamped, base_pose, arm):
         dist = math.hypot(offset_pose_x - x, offset_pose_y - y)
         return dist
 
-    def distance_to_poi_area(self, frame_stamped, arm):
+    def _distance_to_poi_area(self, frame_stamped, arm):
         """
         :return: length of the path the robot would need to drive to place at the given point
         :rtype: int [plan steps]
@@ -135,7 +136,7 @@ def distance_to_poi_area(self, frame_stamped, arm):
             distance = None
         return distance
 
-    def create_marker(self, x, y, z):
+    def _create_marker(self, x, y, z):
         marker = Marker()
         marker.id = len(self.marker_array.markers)
         marker.type = 2
@@ -154,7 +155,7 @@ def create_marker(self, x, y, z):
         marker.lifetime = rospy.Duration(10.0)
         return marker
 
-    def create_selection_marker(self, selected_pose):
+    def _create_selection_marker(self, selected_pose):
         marker = Marker()
         marker.id = len(self.marker_array.markers) + 1
         marker.type = 2
@@ -173,7 +174,7 @@ def create_selection_marker(self, selected_pose):
         marker.lifetime = rospy.Duration(30.0)
         return marker
 
-    def determine_points_of_interest_with_area(self, entity, area):
+    def _determine_points_of_interest_with_area(self, entity, area):
         """ Determines the points of interest using an area
         :type entity: Entity
         :param area: str indicating which volume of the entity to look at
@@ -193,9 +194,9 @@ def determine_points_of_interest_with_area(self, entity, area):
         # Now we're sure to have the correct bounding box
         # Make sure we offset the bottom of the box
         top_z = box.min_corner.z() - 0.04  # 0.04 is the usual offset
-        return self.determine_points_of_interest(entity._pose, top_z, box.bottom_area)
+        return self._determine_points_of_interest(entity._pose, top_z, box.bottom_area)
 
-    def determine_points_of_interest(self, center_frame, z_max, convex_hull):
+    def _determine_points_of_interest(self, center_frame, z_max, convex_hull):
         """
         Determine candidates for place poses
         :param center_frame: kdl.Frame, center of the Entity to place on top of
@@ -243,7 +244,7 @@ def determine_points_of_interest(self, center_frame, z_max, convex_hull):
 
                 points += [fs]
 
-                self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z()))
+                self.marker_array.markers.append(self._create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z()))
 
                 # ToDo: check if still within hull???
                 d += self._spacing