def getPixels(strip: PixelStrip):
num_pixels = strip.numPixels()
all_pixels = []
for i in range(num_pixels):
c = strip.getPixelColorRGB(i)
# all_pixels.append((c.r, c.g, c.b))
all_pixels.append(c)
return all_pixels
class RGBLight():
"Simple wrapper for a RGB light device "
def __init__(self, pin: int, nb_led: int = 5):
self.pixels = PixelStrip(nb_led, pin)
self.pixels.begin()
self._state: bool = False
self.delay: float = 0.01
self._mode: int = MODE_RGB
self.off()
@property
def state(self) -> bool:
return self._state
@state.setter
def state(self, state: bool) -> None:
self._state = state
@property
def mode(self) -> int:
return self._mode
@mode.setter
def mode(self, mode) -> None:
self._mode = mode
def on(self) -> None:
self.state = True
for i in range(self.pixels.numPixels()):
print(i)
self.pixels.setPixelColorRGB(i, 50, 50, 50)
self.pixels.show()
sleep(0.01)
def off(self) -> None:
self.state = False
for i in range(self.pixels.numPixels()):
self.pixels.setPixelColorRGB(i, 0, 0, 0)
self.pixels.show()
sleep(self.delay)
def toggle(self) -> None:
if self.state is True:
self.off()
else:
self.on()
@property
def color(self) -> Tuple[int, int, int]:
if self.mode != MODE_RGB:
raise Exception("Light is not in RGB mode")
color_raw = self.pixels.getPixelColorRGB(0)
color = (color_raw.r, color_raw.g, color_raw.b)
return color
@color.setter
def color(self, color: Tuple[int, int, int]) -> None:
self.state = True
self.mode = MODE_RGB
for i in range(self.pixels.numPixels()):
self.pixels.setPixelColorRGB(i, *color)
self.pixels.show()
sleep(self.delay)
@property
def brightness(self) -> int:
bri = self.pixels.getBrightness()
return bri
@brightness.setter
def brightness(self, value: int) -> None:
self.pixels.setBrightness(value)
@property
def temperature(self) -> int:
if self.mode != MODE_TEMPERATURE:
raise Exception("Light is not in temperature mode")
color_raw = self.pixels.getPixelColorRGB(0)
rgb = (color_raw.r, color_raw.g, color_raw.b)
print(rgb)
for temp in kelvin_table.keys():
if kelvin_table[temp] == rgb:
return temp
return 0
@temperature.setter
def temperature(self, temp: int) -> None:
self.mode = MODE_TEMPERATURE
safe_temp = temp - (temp % 100)
rgb: Tuple[int, int, int] = kelvin_table[safe_temp]
for i in range(self.pixels.numPixels()):
self.pixels.setPixelColorRGB(i, *rgb)
self.pixels.show()
sleep(self.delay)
class LedRingAnimations:
"""
Object which implements control method for the Led ring
"""
def __init__(self):
# - Parameters
self.__is_simulation = rospy.get_param("~simulation_mode")
# LED self.strip configuration:
self.LED_COUNT = rospy.get_param('~led_count') # Number of LED pixels.
self.LED_PIN = rospy.get_param(
'~led_pin') # GPIO pin connected to the pixels (must support PWM!)
self.LED_FREQ_HZ = rospy.get_param(
'~led_freq_hs') # LED signal frequency in hertz (usually 800khz)
self.LED_DMA = rospy.get_param(
'~led_dma') # DMA channel to use for generating signal (try 10)
self.LED_BRIGHTNESS = rospy.get_param(
'~led_brightness') # Set to 0 for darkest and 255 for brightest
self.LED_INVERT = rospy.get_param(
'~led_invert'
) # True to invert the signal (when using NPN transistor level shift)
self.LED_CHANNEL = rospy.get_param('~led_channel')
self.__led_offset = rospy.get_param("~led_offset")
if not self.__is_simulation:
self.LED_STRIP = ws.WS2811_STRIP_GRB
# default param for led ring control methods
self.default_flashing_period = rospy.get_param(
'~default_flashing_period')
self.default_alternate_period = rospy.get_param(
'~default_alternate_period')
self.default_chase_period = rospy.get_param('~default_chase_period')
self.default_colorwipe_period = rospy.get_param(
'~default_colorwipe_period')
self.default_rainbow_period = rospy.get_param(
'~default_rainbow_period')
self.default_rainbowcycle_period = rospy.get_param(
'~default_rainbowcycle_period')
self.default_rainbowchase_period = rospy.get_param(
'~default_rainbowchase_period')
self.default_goup_period = rospy.get_param('~default_goup_period')
self.default_goupanddown_period = rospy.get_param(
'~default_goupanddown_period')
self.default_breath_period = rospy.get_param('~default_breath_period')
self.default_snake_period = rospy.get_param('~default_snake_period')
self.__stop_func = False
self.__animation_lock = Lock()
self._observers = []
self.current_animation_color = BLACK
self.current_animation = LedRingAnimation.NONE
self.set_current_anim_and_color(
self.current_animation,
self.current_animation_color,
)
if not self.__is_simulation:
self.strip = PixelStrip(self.LED_COUNT, self.LED_PIN,
self.LED_FREQ_HZ, self.LED_DMA,
self.LED_INVERT, self.LED_BRIGHTNESS,
self.LED_CHANNEL, self.LED_STRIP)
self.strip.begin()
if self.__is_simulation:
self.led_count = self.LED_COUNT
else:
self.led_count = self.strip.numPixels()
self.off_color = GREY if self.__is_simulation else BLACK
# for real mode
# list used to store the current state of the real led ring, as a list of ColorRGBA objects
self.current_real_led_ring_state = []
self.led_ring_makers = LedRingSimulation(self.led_count)
self.blackout()
def __del__(self):
self.set_and_show_leds(self.off_color)
if not self.__is_simulation:
del self.strip
def stop_animation(self):
"""
Stop iteration (and endless iteration) for functions that perform continous action like alternate_color, and
wait until the previous function is finished.
Used when launched in robot status display mode. Indeed, when used by the user,
the previous function is stopped before, in the start thread function
"""
if self.is_animation_running():
self.__stop_func = True
with self.__animation_lock: # wait the end of the running animation
self.blackout()
def init_animation(self):
self.__stop_func = False
def is_animation_running(self):
return self.__animation_lock.locked()
def was_function_interrupted(self):
return self.__stop_func # if true, function was interrupted
def __play_cycle_animation(self, color_cycle, period, iterations,
animation_function):
# start playing animation :
loop_period = period * 1.0 / len(color_cycle)
next_loop_time = rospy.Time.now()
count = 1 if not iterations else iterations
while count > 0:
for cycle_index in range(len(color_cycle)):
if self.__stop_func:
break
animation_function(color_cycle, cycle_index)
next_loop_time += rospy.Duration(loop_period)
self.__sleep_animation(next_loop_time)
if self.__stop_func:
break
if iterations:
count -= 1
self.blackout()
def none(self):
"""
Turn off leds, in simu and in real, with the "solid" method
"""
self.solid(self.off_color)
def solid(self, color_rgba):
"""
Sets all Leds to a color at once
"""
self.init_animation()
with self.__animation_lock:
self.set_and_show_leds(color_rgba)
mode = LedRingAnimation.NONE if color_rgba == self.off_color else LedRingAnimation.SOLID
self.set_current_anim_and_color(mode, color_rgba)
def custom(self, color_rgba):
"""
Sets all Leds to a color at once
"""
self.init_animation()
with self.__animation_lock:
colors = color_rgba[:self.led_count] if len(
color_rgba) > self.led_count else color_rgba + (
len(color_rgba) - self.led_count) * [BLACK]
for led_id, led_color in enumerate(colors):
# set color led by led
self.set_led(led_id, led_color)
self.show_leds() # display all leds
self.set_current_anim_and_color(LedRingAnimation.CUSTOM)
def set_led_color(self, led_id, color_rgba):
self.init_animation()
with self.__animation_lock:
self.set_led(led_id, color_rgba)
self.show_leds()
self.set_current_anim_and_color(LedRingAnimation.CUSTOM)
def flashing(self, color_rgba, period=None, iterations=0):
"""
Flash a color according to a frequency
"""
def animation_function(anim_color_cycle, anim_cycle_index):
self.set_and_show_leds(anim_color_cycle[anim_cycle_index])
self.init_animation()
# set default frequency
if not period:
period = self.default_flashing_period
# configure the animation
color_cycle = [color_rgba, self.off_color]
# start the animation
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.FLASHING,
color_rgba)
self.__play_cycle_animation(color_cycle, period, iterations,
animation_function)
def alternate(self, color_list_rgba, period=None, iterations=0):
"""
The different colors are alternated one after the other.
If iterations is 0, do it indefinitely
"""
def animation_function(anim_color_cycle, anim_cycle_index):
self.set_current_anim_and_color(LedRingAnimation.ALTERNATE,
anim_color_cycle[anim_cycle_index])
self.set_and_show_leds(anim_color_cycle[anim_cycle_index])
self.init_animation()
# configure the animation
if not period:
period = self.default_alternate_period
color_cycle = color_list_rgba[:]
# start the animation
with self.__animation_lock:
self.__play_cycle_animation(color_cycle, period, iterations,
animation_function)
def chase(self, color_rgba, period=None, iterations=0):
"""
Movie theater light style chaser animation.
If iterations is 0, do it indefinitely
"""
def animation_function(anim_color_cycle, anim_cycle_index):
for led_id in range(self.led_count):
# set color led by led
self.set_led(
led_id, anim_color_cycle[led_id % len(anim_color_cycle) -
anim_cycle_index])
self.show_leds() # display all leds
self.init_animation()
# configure the animation
if not period:
period = self.default_chase_period
color_cycle = [self.off_color, self.off_color, color_rgba]
# start the animation
with self.__animation_lock:
self.set_and_show_leds(self.off_color)
self.set_current_anim_and_color(LedRingAnimation.CHASE, color_rgba)
self.__play_cycle_animation(color_cycle, period, iterations,
animation_function)
def color_wipe(self, color_rgba, duration=None):
"""
Wipe color across, light a Led at a time.
Similar to goUp, but Leds are not turned off at the end.
"""
self.init_animation()
if not duration:
duration = self.default_colorwipe_period
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.COLOR_WIPE,
color_rgba)
self.__wipe_animation(color_rgba, duration)
if self.__stop_func:
self.blackout() # turn off leds
def __wipe_animation(self, color_rgba, duration):
next_loop = rospy.Time.now()
period = rospy.Duration(duration * 1.0 / self.led_count)
for led_id in range(self.led_count):
if self.__stop_func:
break
self.set_led((led_id + self.__led_offset) % self.led_count,
color_rgba)
self.show_leds()
next_loop += period
self.__sleep_animation(next_loop)
def breath_animation(self, color_rgba, duration):
next_loop = rospy.Time.now()
gamma = 0.2 # affects the width of peak (more or less darkness)
beta = 0.5 # shifts the gaussian to be symmetric
nb_steps = 255
anim_period = rospy.Duration(duration * 1.0 / nb_steps)
self.current_animation_color = color_rgba
for counter in range(nb_steps):
if self.__stop_func:
break
# value = 255 * math.sin(2 * math.pi * (counter * 1.0 / smoothness_pts))
factor = math.exp(-(pow(
((counter * 1.0 / nb_steps) - beta) / gamma, 2.0)) / 2.0)
# self.set_brightness(value)
self.set_and_show_leds(
ColorRGBA(self.current_animation_color.r * factor,
self.current_animation_color.g * factor,
self.current_animation_color.b * factor, 0))
next_loop += anim_period
rospy.sleep(next_loop - rospy.Time.now())
def breath(self, color_rgba, period=None, iterations=0):
"""
Leds turn on like a loading circle, and are all turned off at the same time
If iterations is 0, do it indefinitely
"""
self.init_animation()
if period == 0 or period is None:
period = self.default_breath_period
# start playing animation :
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.BREATH,
color_rgba)
if not iterations:
while not self.__stop_func:
self.breath_animation(color_rgba, period)
else:
for _i in range(iterations):
if self.__stop_func:
break
self.breath_animation(color_rgba, period)
self.blackout()
def snake(self, color_rgba, period=None, iterations=0):
def animation_function(anim_color_cycle, anim_cycle_index):
for led_id in range(self.led_count):
# set color led by led
self.set_led(
led_id, anim_color_cycle[(led_id + anim_cycle_index) %
len(anim_color_cycle)])
self.show_leds() # display all leds
self.init_animation()
# configure the animation
if not period:
period = self.default_snake_period
attenuated_color = ColorRGBA(color_rgba.r * 0.5, color_rgba.g * 0.5,
color_rgba.b * 0.5, 0)
snake_length = 10
snake_pattern = [
attenuated_color
] + (snake_length - 2) * [color_rgba] + [
attenuated_color
] + (self.LED_COUNT - snake_length) * [self.off_color]
# start the animation
with self.__animation_lock:
self.set_and_show_leds(self.off_color)
self.set_current_anim_and_color(LedRingAnimation.SNAKE, color_rgba)
self.__play_cycle_animation(snake_pattern, period, iterations,
animation_function)
def go_up(self, color_rgba, period=None, iterations=0):
"""
Leds turn on like a loading circle, and are all turned off at the same time
If iterations is 0, do it indefinitely
"""
def animation(duration):
end_time = rospy.Time.now() + rospy.Duration(duration)
self.__wipe_animation(
color_rgba, duration * self.led_count / (self.led_count + 1))
self.set_and_show_leds(self.off_color)
rospy.sleep(end_time - rospy.Time.now())
self.init_animation()
if period == 0 or period is None:
period = self.default_goup_period
# start playing animation :
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.GO_UP, color_rgba)
if not iterations:
while not self.__stop_func:
animation(period)
else:
for _ in range(iterations):
if self.__stop_func:
break
animation(period)
self.blackout()
def go_up_and_down(self, color_rgba, period=None, iterations=0):
"""
Leds turn on like a loading circle, and turn off the same way
If iterations is 0, do it indefinitely
"""
def animation(duration):
self.__wipe_animation(color_rgba, duration / 2.0)
self.__wipe_animation(self.off_color, duration / 2.0)
self.init_animation()
if period == 0 or period is None:
period = self.default_goupanddown_period
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.GO_UP_AND_DOWN,
color_rgba)
# start playing animation :
if not iterations:
while not self.__stop_func:
animation(period)
else:
for _ in range(iterations):
if self.__stop_func:
break
animation(period)
self.blackout()
def __rainbow_animation(self, duration, animation_function):
next_loop = rospy.Time.now()
anim_period = rospy.Duration(duration / 256.0)
for color_counter in range(256):
if self.__stop_func:
break
animation_function(color_counter)
next_loop += anim_period
rospy.sleep(next_loop - rospy.Time.now())
def rainbow(self, period=None, iterations=0):
"""
Draw rainbow that fades across all Leds at once
If iterations is 0, do it indefinitely
"""
def animation(color_counter):
# for led_id in range(self.led_count):
# self.set_led(led_id, wheel_rgba((led_id + color_counter) & 255))
# self.show_leds()
self.set_and_show_leds(wheel_rgba(color_counter & 255))
self.init_animation()
# configure the animation
if not period:
period = self.default_rainbow_period
with self.__animation_lock:
# no color info in led_ring_status topic, so we just notify that the animation changed
self.set_current_anim_and_color(LedRingAnimation.RAINBOW)
# start playing animation :
if not iterations:
while not self.__stop_func:
self.__rainbow_animation(period, animation)
else:
for _ in range(iterations):
if self.__stop_func:
break
self.__rainbow_animation(period, animation)
self.blackout() # turn off leds after all iterations
def rainbow_cycle(self, period=None, iterations=0):
"""
Draw rainbow that uniformly distributes itself across all Leds
If iterations is 0, do it indefinitely
"""
def animation(color_counter):
for led_id in range(self.led_count):
self.set_led(
led_id,
wheel_rgba(
int((led_id * 256.0 // self.led_count + color_counter))
& 255))
self.show_leds()
self.init_animation()
if not period:
period = self.default_rainbowcycle_period
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.RAINBOW_CYLE)
# start playing animation :
if not iterations:
while not self.__stop_func:
self.__rainbow_animation(period, animation)
else:
for _ in range(iterations):
if self.__stop_func:
break
self.__rainbow_animation(period, animation)
self.blackout() # turn off leds after all iterations
def rainbow_chase(self, period=None, iterations=0):
"""
Rainbow chase animation
If iterations is 0, do it indefinitely
"""
def animation(color_counter):
for led_id in range(self.led_count):
offset = color_counter % 3
if (led_id + offset) % 3 == 0:
self.set_led(led_id,
wheel_rgba((led_id + color_counter) & 255))
else:
self.set_led(led_id, self.off_color) # don't show
self.show_leds()
self.init_animation()
if not period:
period = self.default_rainbowchase_period
with self.__animation_lock:
self.set_current_anim_and_color(LedRingAnimation.RAINBOW_CHASE)
self.set_and_show_leds(self.off_color)
# start playing animation :
if not iterations:
while not self.__stop_func:
self.__rainbow_animation(period, animation)
else:
for _ in range(iterations):
if self.__stop_func:
break
self.__rainbow_animation(period, animation)
self.blackout() # turn off leds after all iterations
def blackout(self):
"""
Black out every led, without stopping previous function
"""
self.set_and_show_leds(self.off_color)
self.set_current_anim_and_color(LedRingAnimation.NONE, BLACK)
def fade(self, color_rgba, duration=3.5, steps=100):
current_color = self.current_animation_color
step_r = (color_rgba.r - current_color.r) / float(steps)
step_g = (color_rgba.g - current_color.g) / float(steps)
step_b = (color_rgba.b - current_color.b) / float(steps)
sleep_duration = duration / float(steps)
for _ in range(steps):
self.current_animation_color = ColorRGBA(
self.current_animation_color.r + step_r,
self.current_animation_color.g + step_g,
self.current_animation_color.b + step_b, 0)
rospy.sleep(sleep_duration)
# - Real Led ring related method
def get_state_list_from_pixel_strip(self):
"""
Return a list of size self.led_count, containing the current rgb color [r, g, b] of each Led in the
real led ring
"""
real_leds_state = []
if not self.__is_simulation:
for i in range(self.led_count):
# read back the state from the library memory buffer
# (can't read directly Led state, they are Write-only)
color_i = self.strip.getPixelColorRGB(i)
real_leds_state.append([color_i.r, color_i.g, color_i.b])
return real_leds_state
# - Generic methods usable for simulation and real robot
def set_led(self, index, color_rgba):
"""
Set the color of a pixel, in simu or in real
"""
if not self.__is_simulation:
color = get_24bits_color_from_msg(color_rgba)
self.strip.setPixelColor(index, color)
self.led_ring_makers.set_one_led_marker(index, color_rgba)
def show_leds(self):
"""
Display all Led's values previously set, in simu or in real
"""
if not self.__is_simulation:
self.strip.show()
# update value of current led state
current_rgb_array_led_state = self.get_state_list_from_pixel_strip(
)
current_color_rgba_led_state = []
# transform a list like [[r, g, b], [r, g, b], ...] to a list like [ColorRGBA, ColorRGBA, ...]
for elem in current_rgb_array_led_state:
current_color_rgba_led_state.append(
get_rgba_color_from_list(elem))
self.set_current_real_leds_state(current_color_rgba_led_state)
self.led_ring_makers.show_led_ring_markers()
def set_and_show_leds(self, color, range_=None, index_delta=0):
"""
Iterate over all leds, set them to the chosen color and show them all at once.
"range_" must be filled only if we want to iterate over a part of the led ring.
"index_delta" is used by "chase" methods only.
"""
if range_ is None:
range_ = self.led_count
for i in range(range_):
self.set_led(i + index_delta, color)
self.show_leds()
elif isinstance(range_, list) and len(range_) == 3:
for i in range(range_[0], range_[1], range_[2]):
self.set_led(i + index_delta, color)
self.show_leds()
def set_brightness(self, brightness):
if not self.__is_simulation:
self.strip.setBrightness(brightness)
def __sleep_animation(self, until_time):
while (until_time - rospy.Time.now()).to_sec() > 0.1:
rospy.sleep(0.1)
if self.__stop_func:
break
else:
rospy.sleep(until_time - rospy.Time.now())
# Observable related methods
def set_current_anim_and_color(self, animation, color=None):
self.current_animation_color = color if color is not None else BLACK
self.current_animation = animation
self.notify_observers()
def set_current_real_leds_state(self, rgb_list):
self.current_real_led_ring_state = rgb_list
def notify_observers(self):
"""
trigger a function in the observer (led ring node) class
"""
for obs in self._observers:
obs.notify_current_anim_and_color(self)
def register_observer(self, observer):
"""
Used to add the led ring node as an observer of this class
"""
self._observers.append(observer)
