srs_object_verification: srs_object_verification: eval

00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('srs_object_verification');
00004 import rospy
00005 
00006 #from srs_semantics_db.srv import *
00007 #from srs_semantics_db.msg import *
00008 #from gdatabase.srv import *
00009 from cob_3d_mapping_msgs.srv import *
00010 from std_msgs.msg import *
00011 from math import sqrt
00012 
00013 
00014 class EvalObjects:
00015   def semantics_db_list_objects(self, class_id):
00016     rospy.wait_for_service('list_all_instances_of_class')
00017     try:
00018       list_objects = rospy.ServiceProxy('list_all_instances_of_class', ListInstancesOfClass)
00019       req = srs_semantics_db.msg.TBoxObject()
00020       req.class_id = class_id
00021       res = list_objects(req)
00022     except rospy.ServiceException, e:
00023       print "Service call failed: %s"%e
00024     object_poses = []
00025     rospy.wait_for_service('get_info_object')
00026     try:
00027       get_object_info = rospy.ServiceProxy('get_info_object', GetInfoObject)
00028       for l in res.list:
00029         object_poses.append(get_object_info(l.object_id).objectPose)
00030     except rospy.ServiceException, e:
00031       print "Service call failed: %s"%e
00032     return object_poses
00033 
00034   def semantics_db_get_object_info(self, object_id):
00035     rospy.wait_for_service('get_info_object')
00036     try:
00037       get_object_info = rospy.ServiceProxy('get_info_object', GetInfoObject)
00038       return get_object_info(object_id)
00039     except rospy.ServiceException, e:
00040       print "Service call failed: %s"%e
00041 
00042   def map_list_objects(self, class_id):
00043     rospy.wait_for_service('table_extraction/get_tables')
00044     try:
00045       get_objects = rospy.ServiceProxy('table_extraction/get_tables', GetTables)
00046       res = get_objects()
00047       return res
00048     except rospy.ServiceException, e:
00049       print "Service call failed: %s"%e
00050 
00051   def verify_table(self, object_pose, table_list_map):
00052     d_min = 10000
00053     d_th = 0.5
00054     for t in table_list_map.tables:
00055       #pose_map = t.params[4:]
00056       d = sqrt((t.table.pose.pose.position.x-object_pose.position.x)**2+(t.table.pose.pose.position.y-object_pose.position.y)**2)
00057       if d < d_min and d < d_th:
00058         d_min = d
00059         closest_table = t.table
00060     if d_min < 10000:
00061       return closest_table
00062     else:
00063       return False
00064 
00065 
00066 if __name__ == "__main__":
00067   class_id =1 #tables
00068   eo = EvalObjects()
00069   object_poses = eo.semantics_db_list_objects(class_id)
00070   for i in range(0,2):
00071     object_to_search = object_poses[i]
00072     print "Searching for object at " + str(object_to_search.position.x) + ", " + str(object_to_search.position.y)
00073     object_list_map = eo.map_list_objects(class_id)
00074     if class_id == 1:
00075       closest_table = eo.verify_table(object_to_search, object_list_map)
00076       if closest_table:
00077         print "table " + str(object_to_search.position.x) + "," + str(object_to_search.position.y) + " found at " + str(closest_table.params[4]) + "," + str(closest_table.params[5])
00078       else:
00079         print "table " + str(object_to_search.position.x) + "," + str(object_to_search.position.y) + " not found"
eval_objects.py
