Go to the documentation of this file.00001 """
00002 Blinks Kobuki LEDS!!!
00003 """
00004 import rospy
00005 import threading
00006 from kobuki_msgs.msg import Led
00007
00008 STATE_OFF = 0
00009 STATE_OK = 1
00010 STATE_ERROR = 2
00011
00012 class LedBlinker(object):
00013
00014 __led1 = '/mobile_base/commands/led1'
00015 __led2 = '/mobile_base/commands/led2'
00016
00017 def __init__(self):
00018 self.rate = rospy.get_param('~rate', 3)
00019
00020 self.pub = {}
00021 self.pub['led1'] = rospy.Publisher(self.__led1, Led, queue_size=1)
00022 self.pub['led2'] = rospy.Publisher(self.__led2, Led, queue_size=1)
00023
00024 self._last_blink_led = 2
00025 self._state = STATE_OFF
00026 self.thread = threading.Thread(target=self._blinker)
00027 self._lock = threading.Lock()
00028
00029 def start(self):
00030 self.thread.start()
00031
00032 def stop(self):
00033 self.thread.join()
00034
00035 def spin(self):
00036 self.thread.start()
00037 rospy.spin()
00038
00039 self.pub['led1'].publish(Led.BLACK)
00040 self.pub['led2'].publish(Led.BLACK)
00041
00042 def _blinker(self):
00043 r = rospy.Rate(self.rate)
00044 while not rospy.is_shutdown():
00045 self._last_blink_led = (self._last_blink_led % 2 ) + 1
00046
00047 self._lock.acquire()
00048 if self._state == STATE_OFF:
00049 led1 = Led.BLACK
00050 led2 = Led.BLACK
00051 elif self._state == STATE_OK:
00052 led1 = Led.BLACK if self._last_blink_led == 1 else Led.GREEN
00053 led2 = Led.GREEN if self._last_blink_led == 1 else Led.BLACK
00054 elif self._state == STATE_ERROR:
00055 led1 = Led.BLACK if self._last_blink_led == 1 else Led.RED
00056 led2 = Led.RED if self._last_blink_led == 1 else Led.BLACK
00057 self._lock.release()
00058
00059 self.pub['led1'].publish(led1)
00060 self.pub['led2'].publish(led2)
00061 r.sleep()
00062
00063 def set_on_error(self):
00064 self._lock.acquire()
00065 self._state = STATE_ERROR
00066 self._lock.release()
00067
00068 def set_on_ok(self):
00069 self._lock.acquire()
00070 self._state = STATE_OK
00071 self._lock.release()
00072
00073 def set_on_off(self):
00074 self._lock.acquire()
00075 self._state = STATE_OFF
00076 self._lock.release()