23 from std_msgs.msg
import ColorRGBA
25 from cob_light.msg
import LightMode, LightModes, SetLightModeAction, SetLightModeGoal
26 from cob_light.srv
import StopLightMode, StopLightModeRequest
28 from simple_script_server
import *
47 self.
num_leds = rospy.get_param(
"~num_leds", 1)
50 if not rospy.has_param(
"~light_components"):
51 rospy.logwarn(
"parameter light_components does not exist on ROS Parameter Server")
56 self.
mode = LightMode()
57 self.mode.priority = 8
62 if not rospy.has_param(
"~sound_components"):
63 rospy.logwarn(
"parameter sound_components does not exist on ROS Parameter Server")
67 self.
sound_critical = rospy.get_param(
"~sound_critical",
"My battery is empty, please recharge now.")
68 self.
sound_warning = rospy.get_param(
"~sound_warning",
"My battery is nearly empty, please consider recharging.")
83 action_server_name = component +
"/set_light" 86 rospy.logdebug(
"waiting for %s action server to start",action_server_name)
87 if not client.wait_for_server(rospy.Duration(5)):
89 rospy.logerr(
"%s action server not ready within timeout, aborting...", action_server_name)
91 rospy.logdebug(
"%s action server ready",action_server_name)
94 goal = SetLightModeGoal()
96 client.send_goal(goal)
97 client.wait_for_result()
98 res = client.get_result()
106 srv_server_name = component +
"/stop_mode" 108 rospy.wait_for_service(srv_server_name, timeout=2)
109 srv_proxy = rospy.ServiceProxy(srv_server_name, StopLightMode)
110 req = StopLightModeRequest()
114 except Exception
as e:
115 rospy.logerr(
"%s service failed: %s",srv_server_name, e)
120 sss.say(component, [text])
128 mode = copy.copy(self.
mode)
129 mode.mode = LightModes.FLASH
130 color = ColorRGBA(1, 0, 0, 1)
132 mode.colors.append(color)
142 mode = copy.copy(self.
mode)
143 mode.mode = LightModes.FLASH
144 color = ColorRGBA(1, 0, 0, 1)
146 mode.colors.append(color)
153 mode = copy.copy(self.
mode)
154 mode.mode = LightModes.FLASH
155 color = ColorRGBA(1, 1, 0, 1)
157 mode.colors.append(color)
164 if self.
is_charging ==
False and self.power_state.charging ==
True:
167 if self.
is_charging ==
True and (self.power_state.charging ==
False 176 rospy.logdebug(
'adjusting leds')
177 mode = copy.copy(self.
mode)
179 leds = int(self.
num_leds * self.power_state.relative_remaining_capacity / 100.)
180 mode.mode = LightModes.CIRCLE_COLORS
181 mode.frequency = 60.0
183 color = ColorRGBA(0.0, 1.0, 0.7, 0.4)
184 for _
in range(leds):
185 mode.colors.append(color)
187 mode.mode = LightModes.BREATH
190 hue = 0.34 * (self.power_state.relative_remaining_capacity / 100.0)
191 rgb = colorsys.hsv_to_rgb(hue, 1, 1)
192 color = ColorRGBA(rgb[0], rgb[1], rgb[2], 1.0)
194 mode.colors.append(color)
200 if __name__ ==
"__main__":
201 rospy.init_node(
"battery_monitor")
def set_light(self, mode, track=False)
def timer_callback(self, event)
relative_remaining_capacity
def power_callback(self, msg)