Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 import roslib; roslib.load_manifest('pr2_clutter_helper')
00032 from pr2_lcs_helper.srv import *
00033 from sensor_msgs.msg import CameraInfo, PointCloud, Image
00034
00035 from tf.msg import tfMessage
00036
00037 import sys
00038
00039
00040 import rospy
00041
00042 NODE_NAME = 'get_camera_info_server'
00043 TOPIC_NAME = "wide_stereo/right/camera_info"
00044
00045 global msg_type, srv_type, response_type
00046 msg_type = CameraInfo
00047 srv_type = GetCameraInfo
00048 resp_type = GetCameraInfoResponse
00049 global_ros_data = None
00050
00051 '''
00052 Comments:
00053 Traps exactly one message from the topic specified.
00054 Subscribes to the topic, but returns as a service request.
00055 '''
00056
00057 def sub_callback(data):
00058 print '[get] callback called'
00059 global global_ros_data
00060 global_ros_data = data
00061
00062 def subscribe_once(topic_name=TOPIC_NAME, once_msg_type=msg_type):
00063 s = rospy.Subscriber(topic_name, once_msg_type, sub_callback)
00064
00065 print 'sleeping now: waiting for topic', topic_name
00066 global global_ros_data
00067 while (not global_ros_data):
00068 rospy.sleep(.1)
00069 if rospy.is_shutdown():
00070 print '\n Forcing exit'
00071 return False;
00072 print 'Returning received message from:', topic_name
00073 s.unregister()
00074
00075 return global_ros_data
00076
00077 def handle_service_request(req):
00078 topic_name = req.topic_name
00079
00080 data = subscribe_once(topic_name, msg_type)
00081
00082 return resp_type( data )
00083
00084
00085 ''' Ex: node_name = my_node, service_name = 'get_camera_info'
00086 '''
00087 def get_one_server(node_name, service_name):
00088 rospy.init_node(node_name)
00089 s = rospy.Service(service_name, srv_type, handle_service_request)
00090 print "Ready to get", request
00091 rospy.spin()
00092
00093
00094
00095
00096
00097 if __name__ == '__main__':
00098 print 'starting get_server.py'
00099
00100 for arg in sys.argv: print '[arg] ', arg
00101
00102 try:
00103 request = sys.argv[1]
00104 print '[get] using msg type of', request
00105 except:
00106 print '[get] using defaults of CameraInfo'
00107 request = 'CameraInfo'
00108 try: default_topic_name = sys.argv[2]
00109 except: print '[get] ignoring default topic name'
00110 try: default_reference_frame = sys.argv[3]
00111 except: print '[get] ignoring default reference frame'
00112
00113 unique_node_name = request + '_server'
00114 unique_service_name = 'get_'+ request
00115 print 'starting service with name: "%s" ' %unique_service_name
00116
00117 msg = {'CameraInfo':CameraInfo, 'Image':Image,
00118 'PointCloud':PointCloud,
00119 'tfMessage': tfMessage}
00120 srv = {'CameraInfo':GetCameraInfo, 'Image':GetImage,
00121 'PointCloud':GetPointCloud,
00122 'tfMessage': GettfMessage}
00123 resp = {'CameraInfo':GetCameraInfoResponse,
00124 'Image':GetImageResponse,
00125 'PointCloud':GetPointCloudResponse,
00126 'tfMessage': GettfMessageResponse}
00127 try:
00128
00129 msg_type = msg[request]
00130 srv_type = srv[request]
00131 resp_type = resp[request]
00132 except:
00133 print '[get service] unable to handle requested service of type: ', request
00134 sys.exit()
00135 get_one_server(unique_node_name, unique_service_name)
00136
00137
00138
00139
00140
00141
00142
00143