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))
