def test_ROSInterruptException(self): from rospy.exceptions import ROSInterruptException, ROSException try: raise ROSInterruptException("test") except ROSException: pass try: raise ROSInterruptException("test") except KeyboardInterrupt: pass
def wait_for_service_D(service, timeout=None): master = rosgraph.Master(rospy.names.get_caller_id()) if timeout == 0: rospy.loginfo( 'timeout must be non-zero value, setting timeout to default value') return False else: resolved_name = rospy.names.resolve_name(service) rospy.loginfo('resolved_name: ' + resolved_name) if timeout: timeout_time = time.time() + timeout while not rospy.core.is_shutdown() and time.time() < timeout_time: try: if contact_service(resolved_name, master, timeout_time - time.time()): return True time.sleep(0.3) except KeyboardInterrupt: rospy.loginfo( 'wait_for_service: received keyboard interrupt') raise except: rospy.loginfo( "wait_for_service(%s): service not actually up failed to contact [%s], will keep trying" % (resolved_name, uri)) return False if rospy.core.is_shutdown(): raise ROSInterruptException("rospy shutdown") else: # raise ROSException("timeout exceeded while waiting for service %s" % resolved_name)\ rospy.loginfo("timeout exceeded while waiting for service %s" % resolved_name) return False else: # timeout is None while not rospy.core.is_shutdown(): try: if contact_service(resolved_name, master): return True time.sleep(0.3) except KeyboardInterrupt: rospy.loginfo( 'wait_for_service: received keyboard interrupt') raise except: rospy.loginfo("wait_for_service(%s): will keep trying" % resolved_name) return False if rospy.core.is_shutdown(): raise ROSInterruptException("rospy shutdown")
def _wait_for(self, condition_fn, timeout=5.0): deadline = rospy.Time.now() + rospy.Duration(timeout) while True: if condition_fn(): return True if rospy.Time.now() > deadline: return False if rospy.core.is_shutdown(): raise ROSInterruptException("rospy shutdown") rospy.rostime.wallsleep(0.01)
def wait_for_service(service, timeout=None): """ Blocks until service is available. Use this in initialization code if your program depends on a service already running. @param service: name of service @type service: str @param timeout: timeout time in seconds, None for no timeout. NOTE: timeout=0 is invalid as wait_for_service actually contacts the service, so non-blocking behavior is not possible. For timeout=0 uses cases, just call the service without waiting. @type timeout: double @raise ROSException: if specified timeout is exceeded @raise ROSInterruptException: if shutdown interrupts wait """ master = rosgraph.Master(rospy.names.get_caller_id()) def contact_service(resolved_name, timeout=10.0): try: uri = master.lookupService(resolved_name) except rosgraph.MasterException: return False addr = rospy.core.parse_rosrpc_uri(uri) if rosgraph.network.use_ipv6(): s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM) else: s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) try: # we always want to timeout just in case we're connecting # to a down service. s.settimeout(timeout) logdebug('connecting to ' + str(addr)) s.connect(addr) h = { 'probe': '1', 'md5sum': '*', 'callerid': rospy.core.get_caller_id(), 'service': resolved_name } rosgraph.network.write_ros_handshake_header(s, h) return True finally: if s is not None: s.close() if timeout == 0.: raise ValueError("timeout must be non-zero") resolved_name = rospy.names.resolve_name(service) first = False if timeout: timeout_t = time.time() + timeout while not rospy.core.is_shutdown() and time.time() < timeout_t: try: if contact_service(resolved_name, timeout_t - time.time()): return time.sleep(0.3) except KeyboardInterrupt: # re-raise rospy.core.logdebug( "wait_for_service: received keyboard interrupt, assuming signals disabled and re-raising" ) raise except: # service not actually up if first: first = False rospy.core.logerr( "wait_for_service(%s): failed to contact [%s], will keep trying" % (resolved_name, uri)) if rospy.core.is_shutdown(): raise ROSInterruptException("rospy shutdown") else: raise ROSException( "timeout exceeded while waiting for service %s" % resolved_name) else: while not rospy.core.is_shutdown(): try: if contact_service(resolved_name): return time.sleep(0.3) except KeyboardInterrupt: # re-raise rospy.core.logdebug( "wait_for_service: received keyboard interrupt, assuming signals disabled and re-raising" ) raise except: # service not actually up if first: first = False rospy.core.logerr( "wait_for_service(%s): failed to contact [%s], will keep trying" % (resolved_name, uri)) if rospy.core.is_shutdown(): raise ROSInterruptException("rospy shutdown")