Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 import rospy
00010 import threading
00011
00012
00013 from .exceptions import ServicePairException
00014
00015
00016
00017
00018
00019
00020 class ServicePairServer(object):
00021 '''
00022 The server side of a pubsub service pair.
00023 '''
00024 __slots__ = [
00025 '_publisher',
00026 '_subscriber',
00027 '_callback',
00028 '_use_threads',
00029
00030 'ServicePairSpec',
00031 'ServicePairRequest',
00032 'ServicePairResponse',
00033 ]
00034
00035
00036
00037
00038
00039 def __init__(self, name, callback, ServicePairSpec, use_threads=False):
00040 '''
00041 @param name : resource name of service pair (e.g. testies for pair topics testies/request, testies/response)
00042 @type str
00043 @param callback : function invoked when a request arrives
00044 @param ServicePairSpec : the pair type (e.g. rocon_service_pair_msgs.msg.TestiesPair)
00045 @type str
00046 '''
00047 self._callback = callback
00048 self._use_threads = use_threads
00049 try:
00050 p = ServicePairSpec()
00051 self.ServicePairSpec = ServicePairSpec
00052 self.ServicePairRequest = type(p.pair_request)
00053 self.ServicePairResponse = type(p.pair_response)
00054 except AttributeError:
00055 raise ServicePairException("Type is not an pair spec: %s" % str(ServicePairSpec))
00056 self._subscriber = rospy.Subscriber(name + "/request", self.ServicePairRequest, self._internal_callback)
00057 self._publisher = rospy.Publisher(name + "/response", self.ServicePairResponse)
00058
00059
00060
00061
00062
00063 def reply(self, request_id, msg):
00064 '''
00065 @param request_id : the request id to associate with this response.
00066 @type uuid_msgs.UniqueID
00067
00068 @param msg : the response
00069 @type ServiceResponse
00070 '''
00071 pair_response = self.ServicePairResponse()
00072 pair_response.id = request_id
00073 pair_response.response = msg
00074 self._publisher.publish(pair_response)
00075
00076
00077
00078
00079
00080 def _internal_callback(self, msg):
00081 '''
00082 @param msg : message returned from the server (with pair id etc)
00083 @type self.ServicePairRequest
00084 '''
00085
00086 if self._use_threads:
00087 thread = threading.Thread(target=self._callback, args=(msg.id, msg.request))
00088 thread.start()
00089 else:
00090 self._callback(msg.id, msg.request)