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