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