$search
00001 #!/usr/bin/python 00002 import roslib 00003 roslib.load_manifest('srs_assisted_detection') 00004 00005 00006 import rospy 00007 00008 from srs_assisted_detection.srv import * 00009 from cob_object_detection_msgs.msg import * 00010 from cob_object_detection_msgs.srv import * 00011 from sensor_msgs.msg import * 00012 from std_msgs.msg import * 00013 00014 00015 detector_response=UiDetectorResponse() 00016 00017 def detectObjectSrv(req): 00018 rospy.loginfo("kam was an") 00019 rospy.wait_for_service('/object_detection/detect_object') 00020 try: 00021 string=String() 00022 string.data=req.object_name.data 00023 rospy.loginfo(string) 00024 00025 srv_get_Objects = rospy.ServiceProxy('/object_detection/detect_object', DetectObjects) 00026 roi=RegionOfInterest() 00027 resp1=srv_get_Objects(req.object_name,roi) 00028 #rospy.loginfo(resp1.object_list.detections[0]) 00029 00030 00031 except rospy.ServiceException, e: 00032 print "Service call failed: %s"%e 00033 00034 global detector_response 00035 if len(resp1.object_list.detections) > 0: 00036 detector_response.object_list.header=resp1.object_list.header 00037 for x in range(len(resp1.object_list.detections)): 00038 detector_response.object_list.detections.insert(x,resp1.object_list.detections[x]) 00039 00040 s.shutdown() 00041 return detector_response 00042 00043 00044 def asisted_Detection_server(): 00045 # rospy.init_node('asisted_Detection_server') 00046 global s 00047 s = rospy.Service('assisted_detection', UiDetector, detectObjectSrv) 00048 rospy.loginfo("Assisted Detection ready.") 00049 s.spin() 00050 return detector_response 00051 00052 if __name__ == "__main__": 00053 00054 asisted_Detection_server()