def _read_ok_byte(self, b, sock): """ Utility for reading the OK-byte/error-message header preceding each message. @param sock: socket connection. Will be read from if OK byte is false and error message needs to be read @type sock: socket.socket @param b: buffer to read from @type b: StringIO """ if b.tell() == 0: return pos = b.tell() b.seek(0) ok = struct.unpack('<B', b.read(1))[0] # read in ok byte b.seek(pos) if not ok: str = self._read_service_error(sock, b) #_read_ok_byte has to reset state of the buffer to #consumed as this exception will bypass rest of #deserialized_messages logic. we currently can't have #multiple requests in flight, so we can keep this simple b.seek(0) b.truncate(0) raise ServiceException("service [%s] responded with an error: %s" % (self.resolved_name, str)) else: # success, set seek point to start of message b.seek(pos)
def _get_service_uri(self, request): """ private routine for getting URI of service to call @param request: request message @type request: L{rospy.Message} """ if not isinstance(request, roslib.message.Message): raise TypeError( "request object is not a valid request message instance") # in order to support more interesting overrides, we only # check that it declares the same ROS type instead of a # stricter class check #if not self.request_class == request.__class__: if not self.request_class._type == request._type: raise TypeError( "request object type [%s] does not match service type [%s]" % (request.__class__, self.request_class)) #TODO: subscribe to service changes #if self.uri is None: if 1: #always do lookup for now, in the future we need to optimize try: try: code, msg, self.uri = roslib.scriptutil.get_master( ).lookupService(rospy.names.get_caller_id(), self.resolved_name) except: raise ServiceException("unable to contact master") if code != 1: logger.error( "[%s]: lookup service failed with message [%s]", self.resolved_name, msg) raise ServiceException("service [%s] unavailable" % self.resolved_name) # validate try: rospy.core.parse_rosrpc_uri(self.uri) except rospy.impl.validators.ParameterInvalid: raise ServiceException( "master returned invalid ROSRPC URI: %s" % self.uri) except socket.error as e: logger.error( "[%s]: socket error contacting service, master is probably unavailable", self.resolved_name) return self.uri
def convert_return_to_response(response, response_class): """ Convert return value of function to response instance. The rules/precedence for this are: 1. If the return type is the same as the response type, no conversion is done. 2. If the return type is a dictionary, it is used as a keyword-style initialization for a new response instance. 3. If the return type is *not* a list type, it is passed in as a single arg to a new response instance. 4. If the return type is a list/tuple type, it is used as a args-style initialization for a new response instance. """ # use this declared ROS type check instead of a direct instance # check, which allows us to play tricks with serialization and # deserialization if isinstance(response, genpy.Message) and response._type == response_class._type: #if isinstance(response, response_class): return response elif type(response) == dict: # kwds response try: return response_class(**response) except AttributeError as e: raise ServiceException("handler returned invalid value: %s"%str(e)) elif response == None: raise ServiceException("service handler returned None") elif type(response) not in [list, tuple]: # single, non-list arg try: return response_class(response) except TypeError as e: raise ServiceException("handler returned invalid value: %s"%str(e)) else: # user returned a list, which has some ambiguous cases. Our resolution is that # all list/tuples are converted to *args try: return response_class(*response) except TypeError as e: raise ServiceException("handler returned wrong number of values: %s"%str(e))
def shutdown(self, reason=''): """ Stop this service @param reason: human-readable shutdown reason @type reason: str """ self.done = True logdebug('[%s].shutdown: reason [%s]'%(self.resolved_name, reason)) try: #TODO: make service manager configurable get_service_manager().unregister(self.resolved_name, self) except Exception as e: logerr("Unable to unregister with master: "+traceback.format_exc()) raise ServiceException("Unable to connect to master: %s"%e)
def call(self, *args, **kwds): """ Call the service. This accepts either a request message instance, or you can call directly with arguments to create a new request instance. e.g.:: add_two_ints(AddTwoIntsRequest(1, 2)) add_two_ints(1, 2) add_two_ints(a=1, b=2) @raise TypeError: if request is not of the valid type (Message) @raise ServiceException: if communication with remote service fails @raise ROSInterruptException: if node shutdown (e.g. ctrl-C) interrupts service call @raise ROSSerializationException: If unable to serialize message. This is usually a type error with one of the fields. """ # convert args/kwds to request message class request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds) # initialize transport if self.transport is None: service_uri = self._get_service_uri(request) dest_addr, dest_port = rospy.core.parse_rosrpc_uri(service_uri) # connect to service transport = TCPROSTransport(self.protocol, self.resolved_name) transport.buff_size = self.buff_size try: transport.connect(dest_addr, dest_port, service_uri) except TransportInitError as e: # can be a connection or md5sum mismatch raise ServiceException("unable to connect to service: %s" % e) self.transport = transport else: transport = self.transport # send the actual request message self.seq += 1 transport.send_message(request, self.seq) try: responses = transport.receive_once() if len(responses) == 0: raise ServiceException("service [%s] returned no response" % self.resolved_name) elif len(responses) > 1: raise ServiceException( "service [%s] returned multiple responses: %s" % (self.resolved_name, len(responses))) except rospy.exceptions.TransportException as e: # convert lower-level exception to exposed type if rospy.core.is_shutdown(): raise rospy.exceptions.ROSInterruptException( "node shutdown interrupted service call") else: raise ServiceException( "transport error completing service call: %s" % (str(e))) finally: if not self.persistent: transport.close() self.transport = None return responses[0]
# check, which allows us to play tricks with serialization and # deserialization if isinstance( response, roslib.message.Message) and response._type == response_class._type: #if isinstance(response, response_class): return response elif type(response) == dict: # kwds response try: return response_class(**response) except AttributeError, e: raise ServiceException("handler returned invalid value: %s" % str(e)) elif response == None: raise ServiceException("service handler returned None") elif type(response) not in [list, tuple]: # single, non-list arg try: return response_class(response) except TypeError, e: raise ServiceException("handler returned invalid value: %s" % str(e)) else: # user returned a list, which has some ambiguous cases. Our resolution is that # all list/tuples are converted to *args try: return response_class(*response) except TypeError, e: raise ServiceException( "handler returned wrong number of values: %s" % str(e))