def receive_loop(self, msgs_callback):
"""
Receive messages until shutdown
@param msgs_callback: callback to invoke for new messages received
@type msgs_callback: fn([msg])
"""
# - use assert here as this would be an internal error, aka bug
logger.debug("receive_loop for [%s]", self.name)
try:
try:
while not self.done and not is_shutdown():
msgs = self.receive_once()
if not self.done and not is_shutdown():
msgs_callback(msgs)
rospydebug("receive_loop[%s]: done condition met, exited loop"%self.name)
except DeserializationError as e:
logerr("[%s] error deserializing incoming request: %s"%self.name, str(e))
rospyerr("[%s] error deserializing incoming request: %s"%self.name, traceback.format_exc())
except:
# in many cases this will be a normal hangup, but log internally
try:
#1467 sometimes we get exceptions due to
#interpreter shutdown, so blanket ignore those if
#the reporting fails
rospydebug("exception in receive loop for [%s], may be normal. Exception is %s",self.name, traceback.format_exc())
except: pass
finally:
if not self.done:
self.close()
def handle(self, transport, header):
"""
Process incoming request. This method should be run in its
own thread. If header['persistent'] is set to 1, method will
block until connection is broken.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param header: headers from client
@type header: dict
"""
if 'persistent' in header and \
header['persistent'].lower() in ['1', 'true']:
persistent = True
else:
persistent = False
if header.get('probe', None) == '1':
#this will likely do more in the future
transport.close()
return
handle_done = False
while not handle_done:
try:
requests = transport.receive_once()
for request in requests:
self._handle_request(transport, request)
if not persistent:
handle_done = True
except rospy.exceptions.TransportTerminated as e:
if not persistent:
logerr("incoming connection failed: %s" % e)
logdebug("service[%s]: transport terminated" %
self.resolved_name)
handle_done = True
transport.close()
def handle(self, transport, header):
"""
Process incoming request. This method should be run in its
own thread. If header['persistent'] is set to 1, method will
block until connection is broken.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param header: headers from client
@type header: dict
"""
if 'persistent' in header and \
header['persistent'].lower() in ['1', 'true']:
persistent = True
else:
persistent = False
if header.get('probe', None) == '1':
#this will likely do more in the future
transport.close()
return
handle_done = False
while not handle_done:
try:
requests = transport.receive_once()
for request in requests:
self._handle_request(transport, request)
if not persistent:
handle_done = True
except rospy.exceptions.TransportTerminated as e:
if not persistent:
logerr("incoming connection failed: %s"%e)
logdebug("service[%s]: transport terminated"%self.resolved_name)
handle_done = True
transport.close()
def _handle_request(self, transport, request):
"""
Process a single incoming request.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param request: Message
@type request: roslib.message.Message
"""
try:
# convert return type to response Message instance
response = convert_return_to_response(self.handler(request),
self.response_class)
self.seq += 1
# ok byte
transport.write_buff.write(struct.pack('<B', 1))
transport.send_message(response, self.seq)
except ServiceException as e:
rospy.core.rospydebug("handler raised ServiceException: %s" % (e))
self._write_service_error(transport,
"service cannot process request: %s" % e)
except Exception as e:
logerr("Error processing request: %s\n%s" %
(e, traceback.print_exc()))
self._write_service_error(transport,
"error processing request: %s" % e)
def receive_loop(self, msgs_callback):
"""
Receive messages until shutdown
@param msgs_callback: callback to invoke for new messages received
@type msgs_callback: fn([msg])
"""
# - use assert here as this would be an internal error, aka bug
logger.debug("receive_loop for [%s]", self.name)
try:
while not self.done and not is_shutdown():
try:
if self.socket is not None:
msgs = self.receive_once()
if not self.done and not is_shutdown():
msgs_callback(msgs, self)
else:
self._reconnect()
except TransportException as e:
# set socket to None so we reconnect
try:
if self.socket is not None:
try:
self.socket.shutdown()
except:
pass
finally:
self.socket.close()
except:
pass
self.socket = None
except DeserializationError as e:
#TODO: how should we handle reconnect in this case?
logerr(
"[%s] error deserializing incoming request: %s" %
self.name, str(e))
rospyerr(
"[%s] error deserializing incoming request: %s" %
self.name, traceback.format_exc())
except:
# in many cases this will be a normal hangup, but log internally
try:
#1467 sometimes we get exceptions due to
#interpreter shutdown, so blanket ignore those if
#the reporting fails
rospydebug(
"exception in receive loop for [%s], may be normal. Exception is %s",
self.name, traceback.format_exc())
except:
pass
rospydebug("receive_loop[%s]: done condition met, exited loop" %
self.name)
finally:
if not self.done:
self.close()
def _connect_topic_thread(self, topic, uri):
try:
code, msg, _ = self.handler._connect_topic(topic, uri)
if code != 1:
logerr(
"Unable to connect subscriber to publisher [%s] for topic [%s]: %s",
uri, topic, msg)
except Exception as e:
if not is_shutdown():
logerr(
"Unable to connect to publisher [%s] for topic [%s]: %s" %
(uri, topic, traceback.format_exc()))
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 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 start_server(self):
"""
Starts the TCP socket server if one is not already running
"""
if self.tcp_ros_server:
return
with self.lock:
try:
if not self.tcp_ros_server:
self.tcp_ros_server = TCPServer(self._tcp_server_callback, self.port)
self.tcp_ros_server.start()
except Exception as e:
self.tcp_ros_server = None
logerr("unable to start TCPROS server: %s\n%s"%(e, traceback.format_exc()))
return 0, "unable to establish TCPROS server: %s"%e, []
def notify_removed(self, resolved_name, data_type_or_uri, reg_type):
"""
@param resolved_name: resolved_topic/service name
@type resolved_name: str
@param data_type_or_uri: topic type or service uri
@type data_type_or_uri: str
@param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV}
@type reg_type: str
"""
with self.lock:
for l in self.listeners:
try:
l.reg_removed(resolved_name, data_type_or_uri, reg_type)
except Exception as e:
logerr("error notifying listener of removal: %s"%traceback.format_exc(e))
def notify_added(self, resolved_name, data_type, reg_type):
"""
@param resolved_name: topic/service name
@type resolved_name: str
@param data_type: topic/service type
@type data_type: str
@param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV}
@type reg_type: str
"""
with self.lock:
for l in self.listeners:
try:
l.reg_added(resolved_name, data_type, reg_type)
except Exception as e:
logerr(traceback.format_exc(e))
def start_server(self):
"""
Starts the TCP socket server if one is not already running
"""
if self.tcp_ros_server:
return
with self.lock:
try:
if not self.tcp_ros_server:
self.tcp_ros_server = TCPServer(self._tcp_server_callback, self.port)
self.tcp_ros_server.start()
except Exception as e:
self.tcp_ros_server = None
logerr("unable to start TCPROS server: %s\n%s"%(e, traceback.format_exc()))
return 0, "unable to establish TCPROS server: %s"%e, []
def start_server(self, port=0):
"""
Starts the TCP socket server if one is not already running
@param port: port number to bind to (default 0/any)
@type port: int
"""
if self.tcp_ros_server:
return
with self.lock:
try:
if not self.tcp_ros_server:
self.tcp_ros_server = TCPServer(self._tcp_server_callback, port)
self.tcp_ros_server.start()
except Exception as e:
self.tcp_ros_server = None
logerr("unable to start TCPROS server: %s\n%s"%(e, traceback.format_exc()))
return 0, "unable to establish TCPROS server: %s"%e, []
def start_server(self, port=0):
"""
Starts the TCP socket server if one is not already running
@param port: port number to bind to (default 0/any)
@type port: int
"""
if self.tcp_ros_server:
return
with self.lock:
try:
if not self.tcp_ros_server:
self.tcp_ros_server = TCPServer(self._tcp_server_callback,
port)
self.tcp_ros_server.start()
except Exception as e:
self.tcp_ros_server = None
logerr("unable to start TCPROS server: %s\n%s" %
(e, traceback.format_exc()))
return 0, "unable to establish TCPROS server: %s" % e, []
def _handle_request(self, transport, request):
"""
Process a single incoming request.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param request: Message
@type request: genpy.Message
"""
try:
# convert return type to response Message instance
response = convert_return_to_response(self.handler(request), self.response_class)
self.seq += 1
# ok byte
transport.write_buff.write(struct.pack('<B', 1))
transport.send_message(response, self.seq)
except ServiceException as e:
rospy.core.rospydebug("handler raised ServiceException: %s"%(e))
self._write_service_error(transport, "service cannot process request: %s"%e)
except Exception as e:
logerr("Error processing request: %s\n%s"%(e,traceback.print_exc()))
self._write_service_error(transport, "error processing request: %s"%e)
def receive_loop(self, msgs_callback):
"""
Receive messages until shutdown
@param msgs_callback: callback to invoke for new messages received
@type msgs_callback: fn([msg])
"""
# - use assert here as this would be an internal error, aka bug
logger.debug("receive_loop for [%s]", self.name)
try:
try:
while not self.done and not is_shutdown():
msgs = self.receive_once()
if not self.done and not is_shutdown():
msgs_callback(msgs)
rospydebug(
"receive_loop[%s]: done condition met, exited loop" %
self.name)
except DeserializationError, e:
logerr(
"[%s] error deserializing incoming request: %s" %
self.name, str(e))
rospyerr(
"[%s] error deserializing incoming request: %s" %
self.name, traceback.format_exc())
except:
# in many cases this will be a normal hangup, but log internally
try:
#1467 sometimes we get exceptions due to
#interpreter shutdown, so blanket ignore those if
#the reporting fails
rospydebug(
"exception in receive loop for [%s], may be normal. Exception is %s",
self.name, traceback.format_exc())
except:
pass
def error_handler(self, e, exc_type, exc_value, tb):
logerr("Error processing request: %s\n%s" %
(e, traceback.format_exception(exc_type, exc_value, tb)))
def start(self, uri, master_uri):
"""
Start the RegManager. This should be passed in as an argument to a thread
starter as the RegManager is designed to spin in its own thread
@param uri: URI of local node
@type uri: str
@param master_uri: Master URI
@type master_uri: str
"""
self.registered = False
self.master_uri = master_uri
self.uri = uri
first = True
tm = get_topic_manager()
sm = get_service_manager()
ns = get_namespace()
caller_id = get_caller_id()
if not master_uri or master_uri == uri:
registered = True
master = None
else:
registered = False
master = xmlrpcapi(master_uri)
self.logger.info("Registering with master node %s", master_uri)
while not registered and not is_shutdown():
try:
try:
# prevent TopicManager and ServiceManager from accepting registrations until we are done
tm.lock.acquire()
sm.lock.acquire()
pub, sub, srv = tm.get_publications(), tm.get_subscriptions(), sm.get_services()
for resolved_name, data_type in pub:
self.logger.info("Registering publisher topic [%s] type [%s] with master", resolved_name, data_type)
code, msg, val = master.registerPublisher(caller_id, resolved_name, data_type, uri)
if code != 1:
logfatal("cannot register publication topic [%s] with master: %s"%(resolved_name, msg))
signal_shutdown("master/node incompatibility with register publisher")
for resolved_name, data_type in sub:
self.logger.info("registering subscriber topic [%s] type [%s] with master", resolved_name, data_type)
code, msg, val = master.registerSubscriber(caller_id, resolved_name, data_type, uri)
if code != 1:
logfatal("cannot register subscription topic [%s] with master: %s"%(resolved_name, msg))
signal_shutdown("master/node incompatibility with register subscriber")
else:
self.publisher_update(resolved_name, val)
for resolved_name, service_uri in srv:
self.logger.info("registering service [%s] uri [%s] with master", resolved_name, service_uri)
code, msg, val = master.registerService(caller_id, resolved_name, service_uri, uri)
if code != 1:
logfatal("cannot register service [%s] with master: %s"%(resolved_name, msg))
signal_shutdown("master/node incompatibility with register service")
registered = True
# Subscribe to updates to our state
get_registration_listeners().add_listener(self)
finally:
sm.lock.release()
tm.lock.release()
if pub or sub:
logdebug("Registered [%s] with master node %s", caller_id, master_uri)
else:
logdebug("No topics to register with master node %s", master_uri)
except Exception as e:
if first:
# this use to print to console always, arguable whether or not this should be subjected to same configuration options as logging
logerr("Unable to immediately register with master node [%s]: master may not be running yet. Will keep trying."%master_uri)
first = False
time.sleep(0.2)
self.registered = True
self.run()
@type request: roslib.message.Message
"""
try:
# convert return type to response Message instance
response = convert_return_to_response(self.handler(request),
self.response_class)
self.seq += 1
# ok byte
transport.write_buff.write(struct.pack('<B', 1))
transport.send_message(response, self.seq)
except ServiceException, e:
rospy.core.rospydebug("handler raised ServiceException: %s" % (e))
self._write_service_error(transport,
"service cannot process request: %s" % e)
except Exception, e:
logerr("Error processing request: %s\n%s" %
(e, traceback.print_exc()))
self._write_service_error(transport,
"error processing request: %s" % e)
def handle(self, transport, header):
"""
Process incoming request. This method should be run in its
own thread. If header['persistent'] is set to 1, method will
block until connection is broken.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param header: headers from client
@type header: dict
"""
if 'persistent' in header and \
header['persistent'].lower() in ['1', 'true']:
def error_handler(self, e, exc_type, exc_value, tb):
logerr("Error processing request: %s\n%s" % (e, traceback.format_exception(exc_type, exc_value, tb)))
def receive_loop(self, msgs_callback):
"""
Receive messages until shutdown
@param msgs_callback: callback to invoke for new messages received
@type msgs_callback: fn([msg])
"""
# - use assert here as this would be an internal error, aka bug
logger.debug("receive_loop for [%s]", self.name)
# Start a callback thread to process the callbacks. This way the
# receive loop can continue calling recv() while a long-running
# callback is running.
try:
self.msg_queue = []
self.msg_queue_lock = threading.Lock()
self.msg_queue_condition = threading.Condition(self.msg_queue_lock)
callback_thread = threading.Thread(
target=self.callback_loop,
args=(msgs_callback,))
callback_thread.start()
while not self.done and not is_shutdown():
try:
if self.socket is not None:
msgs = self.receive_once()
if not self.done and not is_shutdown():
with self.msg_queue_lock:
self.msg_queue += msgs
if self.protocol.queue_size is not None:
self.msg_queue = self.msg_queue[-self.protocol.queue_size:]
self.msg_queue_condition.notify()
else:
self._reconnect()
except TransportException as e:
# set socket to None so we reconnect
try:
if self.socket is not None:
try:
self.socket.shutdown()
except:
pass
finally:
self.socket.close()
except:
pass
self.socket = None
except DeserializationError as e:
#TODO: how should we handle reconnect in this case?
logerr("[%s] error deserializing incoming request: %s"%(self.name, str(e)))
rospyerr("[%s] error deserializing incoming request: %s"%(self.name, traceback.format_exc()))
except:
# in many cases this will be a normal hangup, but log internally
try:
#1467 sometimes we get exceptions due to
#interpreter shutdown, so blanket ignore those if
#the reporting fails
rospydebug("exception in receive loop for [%s], may be normal. Exception is %s",self.name, traceback.format_exc())
except: pass
with self.msg_queue_lock:
self.msg_queue_condition.notify()
callback_thread.join()
rospydebug("receive_loop[%s]: done condition met, exited loop"%self.name)
finally:
if not self.done:
self.close()
