Go to the documentation of this file.00001 import rospy
00002 from rospy.impl.tcpros_base import DEFAULT_BUFF_SIZE
00003 from Queue import Queue, Empty as EmptyQueueException
00004 from threading import Condition
00005
00006
00007 callback_queue = Queue()
00008
00009 def spin():
00010 '''
00011 This spinner is used in place of the rospy.spin() method. Whereas
00012 rospy.spin() simply waits for rospy.is_shutdown(), this spinner processes
00013 the callback queue until rospy.is_shutdown() is true. The callback
00014 queue is thread-safe, so for a multi-threaded queue processor, call this
00015 spin method in multiple threads.
00016 '''
00017 while not rospy.is_shutdown():
00018 try:
00019 condition = callback_queue.get(block=True, timeout=0.1)
00020 except EmptyQueueException:
00021 pass
00022 else:
00023 with condition:
00024 condition.notify()
00025 condition.wait()
00026
00027
00028 rospy.spin = spin
00029
00030 def single_threaded(callback):
00031 '''
00032 This decorator can be used on service or subscriber callbacks. A callback
00033 wrapped with this decorator will put a condition on the callback_queue and
00034 then wait its turn before executing the decorated callback.
00035 '''
00036
00037 if hasattr(callback, 'single_threaded') and callback.single_threaded:
00038 return callback
00039
00040 def wrapped_callback(*args, **kwds):
00041
00042 condition = Condition()
00043 with condition:
00044 callback_queue.put(condition)
00045
00046 condition.wait()
00047
00048 try:
00049 ret = callback(*args, **kwds)
00050 finally:
00051 condition.notify()
00052 return ret
00053 wrapped_callback.single_threaded = True
00054 return wrapped_callback
00055
00056 def service_wrapper(func):
00057 '''
00058 This is a decorator for functions that handle ROS service calls.
00059 It catches unhandled exceptions and reports them cleanly as errors
00060 on the ROS console.
00061 '''
00062 def wrapper(*args, **kwds):
00063 try:
00064 return func(*args, **kwds)
00065 except:
00066 rospy.logerr(traceback.format_exc())
00067 return None
00068 return wrapper
00069
00070 class Service(rospy.Service):
00071 def __init__(self, name, service_class, handler,
00072 buff_size=DEFAULT_BUFF_SIZE, error_handler=None,
00073 asynchronous=False):
00074 if not asynchronous:
00075 handler = single_threaded(handler)
00076 super(Service, self).__init__(name, service_class, handler, buff_size,
00077 error_handler)
00078
00079 class Subscriber(rospy.Subscriber):
00080 def __init__(self, name, data_class, callback=None, callback_args=None,
00081 queue_size=None, buff_size=DEFAULT_BUFF_SIZE,
00082 tcp_nodelay=False, asynchronous=False):
00083 if not asynchronous:
00084 callback = single_threaded(callback)
00085 super(Subscriber, self).__init__(name, data_class, callback,
00086 callback_args, queue_size, buff_size, tcp_nodelay)
00087
00088 class Timer(rospy.Timer):
00089 def __init__(self, period, callback, oneshot=False, asynchronous=False):
00090 if not asynchronous:
00091 callback = single_threaded(callback)
00092 super(Timer, self).__init__(period, callback, oneshot)