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