00001
00002
00003
00004
00005
00006
00007
00008
00009 import threading
00010 import subprocess
00011
00012
00013 import rospy
00014 from rocon_device_msgs.msg import HueState, Hue, HueArray
00015
00016
00017 from rocon_python_hue import Bridge
00018 from rocon_python_hue import PhueRegistrationException, PhueException
00019
00020
00021 class RoconBridge():
00022 MAX_HUE = 65535
00023 MAX_SAT = 255
00024 MAX_BRI = 255
00025
00026 def __init__(self, hue_ip='127.0.0.1'):
00027 self.name = 'ros_hue'
00028 self.ip = hue_ip
00029 self.bridge = Bridge()
00030 self.bridge.set_ip_address(self.ip)
00031 self.hue_list_publisher = rospy.Publisher('list_hue', HueArray, latch=True, queue_size=10)
00032
00033 rospy.Subscriber('set_hue', Hue, self.set_hue)
00034
00035 self.checker_th = threading.Thread(target=self.hue_checker)
00036 self.is_checking = True
00037 self.retry_cnt = 0
00038 self.retry_max_cnt = 5
00039 self.checker_th.start()
00040 self.string2color = {}
00041 self.hues = None
00042
00043 self._init_color_lookup_table()
00044
00045 def _init_color_lookup_table(self):
00046 hue_angle = lambda angle: self.MAX_HUE * angle / 360
00047
00048 self.string2color['OFF'] = (0, 0, 0, False)
00049 self.string2color['WHITE'] = (hue_angle(0), 0, self.MAX_BRI, True)
00050 self.string2color['RED'] = (hue_angle(360), self.MAX_SAT, self.MAX_BRI, True)
00051 self.string2color['GREEN'] = (hue_angle(140), self.MAX_SAT, self.MAX_BRI, True)
00052 self.string2color['BLUE'] = (hue_angle(260), self.MAX_SAT, self.MAX_BRI, True)
00053 self.string2color['YELLOW'] = (hue_angle(130), self.MAX_SAT, self.MAX_BRI, True)
00054 self.string2color['ORANGE'] = (hue_angle(70), self.MAX_SAT, self.MAX_BRI, True)
00055 self.string2color['MAGENTA'] = (hue_angle(307), self.MAX_SAT, self.MAX_BRI, True)
00056 self.string2color['VIOLET'] = (hue_angle(280), self.MAX_SAT, self.MAX_BRI, True)
00057
00058 def hue_checker(self):
00059 while self.is_checking and not rospy.is_shutdown():
00060 if self.ping_checker():
00061 if not self.bridge.is_connect:
00062 try:
00063 self.bridge.connect()
00064 except PhueRegistrationException as e:
00065 self.logwarn_ex(e.message)
00066 except PhueException as e:
00067 self.logwarn_ex(e.message)
00068 else:
00069 self.loginfo('bridge connect')
00070 self.retry_cnt = 0
00071 self.bridge.is_connect = True
00072 else:
00073 self.bulb_checker()
00074 else:
00075 self.bridge.is_connect = False
00076 if self.retry_cnt < self.retry_max_cnt:
00077 self.retry_cnt += 1
00078 elif self.retry_cnt == self.retry_max_cnt:
00079 self.retry_cnt += 1
00080 self.loginfo('No more display log regarding retrying to connect to bridge')
00081 self.loginfo_ex('bridge not connect %s' % self.ip)
00082 rospy.sleep(5)
00083
00084 def ping_checker(self):
00085 response = subprocess.call(['ping', '-c', '1', self.ip], stdout=subprocess.PIPE,)
00086 if response == 0:
00087 return True
00088 else:
00089 return False
00090
00091 def bulb_checker(self):
00092 try:
00093 light_ids = self.bridge.get_light_objects(mode='id')
00094 hues = HueArray()
00095 for light_id in light_ids:
00096 state = self.bridge.get_light(light_id)
00097 if not state:
00098 continue
00099 elif state is not '':
00100 hue = Hue()
00101 hue.light_id = light_id
00102 hue.name = state['name']
00103 hue.state.on = state['state']['on']
00104 hue.state.hue = state['state']['hue']
00105 hue.state.sat = state['state']['sat']
00106 hue.state.bri = state['state']['bri']
00107 hue.state.reachable = state['state']['reachable']
00108 hues.hue_list.append(hue)
00109 self.hues = hues
00110 except Exception, e:
00111 self.logwarn_ex('failed. Reason:%s' % str(e))
00112 else:
00113 self.hue_list_publisher.publish(self.hues)
00114
00115 def set_hue(self, data):
00116 self.loginfo(str(data.light_id) + ': ' + str(data.state.color))
00117 if self.bridge.is_connect:
00118 state = self.bridge.get_light(data.light_id)
00119 if data.state.color:
00120 (h, s, v, on) = self.get_color_from_string(data.state.color)
00121 if not state['state']['on']:
00122 state['on'] = on
00123 state['on'] = on
00124 state['hue'] = h
00125 state['sat'] = s
00126 state['bri'] = v
00127 else:
00128 if not state['state']['on']:
00129 state['on'] = on
00130 if data.state.hue:
00131 state['on'] = True
00132 state['hue'] = data.state.hue
00133 if data.state.sat:
00134 state['on'] = True
00135 state['sat'] = data.state.sat
00136 if data.state.bri:
00137 state['on'] = True
00138 state['bri'] = data.state.bri
00139 self.bridge.set_light([data.light_id], state)
00140
00141 def get_color_from_string(self, color):
00142 try:
00143 return self.string2color[color]
00144 except KeyError as e:
00145 self.loginfo('Unsupported Color! Set it to WHITE')
00146 return self.string2color['WHITE']
00147
00148 def loginfo(self, msg):
00149 rospy.loginfo('Rocon Hue : ' + str(msg))
00150
00151 def logwarn(self, msg):
00152 rospy.logwarn('Rocon Hue : ' + str(msg))
00153
00154 def loginfo_ex(self, msg):
00155 if self.retry_cnt < self.retry_max_cnt:
00156 rospy.loginfo('Rocon Hue : ' + str(msg))
00157
00158 def logwarn_ex(self, msg):
00159 if self.retry_cnt < self.retry_max_cnt:
00160 rospy.logwarn('Rocon Hue : ' + str(msg))
00161
00162 def spin(self):
00163 if self.ip is not None:
00164 while not rospy.is_shutdown():
00165 try:
00166 rospy.sleep(0.1)
00167 except:
00168 break
00169 self.is_checking = False
00170 self.checker_th.join(1)
00171 else:
00172 self.logwarn('Must set hue ip')