def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", parse=True): 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) try: entity_infos = self._ed_simple_query_srv(query).entities entities = map(from_entity_info, entity_infos) except Exception, 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("L____> [%s]" % e) return []
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, 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={}, type={}, center_point={}, radius={}, ignore_z={})".format( id, type, str(center_point), str(radius), ignore_z)) rospy.logerr("L____> [%s]" % e) return [] return entities
def get_object(self): rospy.wait_for_service('/ed/kinect/update') self.update_client = rospy.ServiceProxy('/ed/kinect/update', Update) self.request_update = UpdateRequest() self.request_update.area_description = "on_top_of dinner-table" self.result_update = self.update_client(self.request_update) print "Found IDs: " % self.result_update.new_ids rospy.wait_for_service('/ed/simple_query') self.query_client = rospy.ServiceProxy('/ed/simple_query', SimpleQuery) self.query_request = SimpleQueryRequest() self.query_request.id = self.result_update.new_ids[0] self.query_response = self.query_client(self.query_request) rospy.wait_for_service('/ed/simple_query') self.ed_simple = rospy.ServiceProxy('/ed/simple_query', SimpleQuery) simpleRequest = SimpleQueryRequest() simpleRequest.id = 'dinner-table' self.table = self.ed_simple(simpleRequest).entities[0] print self.table.pose