added emotion expression

CMakeLists.txt

@@ -162,6 +162,7 @@ catkin_install_python(PROGRAMS
    nodes/visualiser
    scripts/play_animation.py
    scripts/reset_display.py
+   scripts/build_still_ensemble.py
    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

scripts/build_still_ensemble.py

+#!/usr/bin/env python
+
+import argparse
+
+from visualising.expression.tool import Tool
+from visualising.communication.arduino import Arduino
+from visualising.communication.channel.connection import Connection
+
+parser = argparse.ArgumentParser(description="script to play an animation")
+parser.add_argument("-p", "--port", help="port to which the Arduino is connected", type=str, default="/dev/ttyUSB0")
+parser.add_argument("-b", "--baud", help="baud rate of the connection", type=int, default=57600)
+parser.add_argument("-f", "--file", help="file in the library directory to be played", type=str, required=True)
+parser.add_argument("-t", "--time", help="time between ensembles", type=int, default=100)
+args = vars(parser.parse_args())
+
+port = args["port"]
+baud = args["baud"]
+file = args["file"]
+time = args["time"]
+
+color = [10, 10, 10]
+
+arduino = Arduino(Connection(port, baud))
+
+ensembles = Tool.create_ensembles(file)
+
+arduino.stream_animation(Tool.build_still_ensemble_parallel(ensembles[0], time, color))
+

scripts/play_animation.py

@@ -4,7 +4,6 @@ import argparse
 
 from visualising.expression.tool import Tool
 from visualising.communication.arduino import Arduino
-from visualising.communication.illustration.animation import Animation
 from visualising.communication.channel.connection import Connection
 
 parser = argparse.ArgumentParser(description="script to play an animation")

src/visualising/communication/illustration/frame.py

@@ -10,6 +10,12 @@ class Frame:
 
         self.pixels = pixels
 
+    def is_colored(self, pixel):
+        if not 0 <= pixel <= 15:
+            raise ValueError("Pixel index must be an integer between 0 and 15!")
+
+        return self.pixels[pixel].r + self.pixels[pixel].g + self.pixels[pixel].b != 0
+
     def adjust_brightness(self, pixel, factor):
         if not 0 <= pixel <= 15:
             raise ValueError("Pixel index must be an integer between 0 and 15!")

src/visualising/expression/expression.py

+import time
 import numpy as np
 
+from visualising.expression.tool import Tool
+
+
 class Emotion:
     def __init__(self, arduino):
         self.arduino = arduino
+        self.playback_start = 0
 
     @staticmethod
     def state_to_color(state):
-        red = np.array([255, 0, 0])
-        white = np.array([255, 255, 255])
+        red = np.array([25, 0, 0])
+        white = np.array([25, 25, 25])
 
         return (1 - state) * white + state * red
 
     def react(self, state):
-        color = Emotion.state_to_color(state)
-        if state > 0.7:
+        if time().time - playback_start > 5:
+
+            color = Emotion.state_to_color(state[0])
+
+            if state[0] > 0.7:
                 self.happy(color)
             else:
-            if state > 0.3:
+                if state[0] > 0.3:
                     self.ok(color)
                 else:
                     self.angry(color)
 
     def happy(self, color):
-        
-
+        ensembles = Tool.create_ensembles("expression_happy.txt")
+        self.arduino.stream_animation(Tool.build_still_ensemble_parallel(ensembles[0], 50, color))
+        self.playback_start = time.time()
 
     def ok(self, color):
+        ensembles = Tool.create_ensembles("expression_ok.txt")
+        self.arduino.stream_animation(Tool.build_still_ensemble_parallel(ensembles[0], 50, color))
+        self.playback_start = time.time()
 
+    def angry(self, color):
+        ensembles = Tool.create_ensembles("expression_angry.txt")
+        self.arduino.stream_animation(Tool.build_still_ensemble_parallel(ensembles[0], 50, color))
+        self.playback_start = time.time()
 
 
-    def angry(self, color):
+class State:
+    def __init__(self, arduino):
+        self.arduino = arduino
+        self.playback_start = 0
+
+    def react(self, state):
+        pass
 

src/visualising/expression/tool.py

@@ -15,7 +15,7 @@ class Tool:
         pass
 
     @staticmethod
-    def read_raw_animation(filename):
+    def read_raw_ensembles(filename):
         def read_raw_frame(line):
             raw_frame = []
             for value in line.split(","):
@@ -44,7 +44,7 @@ class Tool:
         return raw_ensembles
 
     @staticmethod
-    def create_animation(filename, ensemble_time=0, num_iter=1):
+    def create_ensembles(filename):
         def create_frame(raw_pixels):
             if len(raw_pixels) != 48:
                 raise ValueError("Frame must have exactly 48 values!")
@@ -62,7 +62,7 @@ class Tool:
 
             return Frame(pixels)
 
-        raw_ensembles = Tool.read_raw_animation(filename)
+        raw_ensembles = Tool.read_raw_ensembles(filename)
 
         ensembles = []
         for raw_ensemble in raw_ensembles:
@@ -74,7 +74,80 @@ class Tool:
 
             ensembles.append(Ensemble(l_frame, r_frame))
 
-        return Animation(ensembles, ensemble_time, num_iter)
+        return ensembles
+
+    @staticmethod
+    def create_animation(filename, ensemble_time, iterations):
+        ensembles = Tool.create_ensembles(filename)
+        return Animation(ensembles, ensemble_time, iterations)
+
+    @staticmethod
+    def build_still_ensemble(still_ensemble, ensemble_time, color):
+        r = color[0]
+        g = color[1]
+        b = color[2]
+
+        ensembles = []
+        for i in range(32):
+            ensemble = Tool.create_empty_ensemble()
+            for j in range(i + 1):
+                if j < 16:
+                    is_colored = still_ensemble.l_frame.is_colored(j)
+                else:
+                    is_colored = still_ensemble.r_frame.is_colored(j % 16)
+
+                if is_colored or j == i:
+                    ensemble = Tool.set_pixel_color(ensemble, j, r, g, b)
+
+            ensembles.append(ensemble)
+
+        ensemble = Tool.create_empty_ensemble()
+        for k in range(32):
+            if k < 16:
+                is_colored = still_ensemble.l_frame.is_colored(k)
+            else:
+                is_colored = still_ensemble.r_frame.is_colored(k % 16)
+
+            if is_colored:
+                ensemble = Tool.set_pixel_color(ensemble, k, r, g, b)
+
+        ensembles.append(ensemble)
+
+        return Animation(ensembles, ensemble_time, 1)
+
+    @staticmethod
+    def build_still_ensemble_parallel(still_ensemble, ensemble_time, color):
+        r = color[0]
+        g = color[1]
+        b = color[2]
+
+        ensembles = []
+        for i in range(16):
+            ensemble = Tool.create_empty_ensemble()
+            for j in range(i + 1):
+                if still_ensemble.l_frame.is_colored(j):
+                    ensemble = Tool.set_pixel_color(ensemble, j, r, g, b)
+                if still_ensemble.r_frame.is_colored(j):
+                    ensemble = Tool.set_pixel_color(ensemble, 16 + j, r, g, b)
+                if j == i:
+                    ensemble = Tool.set_pixel_color(ensemble, j, r, g, b)
+                    ensemble = Tool.set_pixel_color(ensemble, 16 + j, r, g, b)
+
+            ensembles.append(ensemble)
+
+        ensemble = Tool.create_empty_ensemble()
+        for k in range(32):
+            if k < 16:
+                is_colored = still_ensemble.l_frame.is_colored(k)
+            else:
+                is_colored = still_ensemble.r_frame.is_colored(k % 16)
+
+            if is_colored:
+                ensemble = Tool.set_pixel_color(ensemble, k, r, g, b)
+
+        ensembles.append(ensemble)
+
+        return Animation(ensembles, ensemble_time, 1)
 
     @staticmethod
     def merge_frames(l_frames, r_frames):
@@ -94,56 +167,23 @@ class Tool:
 
     @staticmethod
     def create_empty_ensemble():
-        frame1 = [0] * 16
-        frame2 = [0] * 16
-        return [frame1, frame2]
+        l_pixels = []
+        r_pixels = []
+        for _ in range(16):
+            l_pixels.append(Pixel(0, 0, 0))
+            r_pixels.append(Pixel(0, 0, 0))
+
+        return Ensemble(Frame(l_pixels), Frame(r_pixels))
 
     @staticmethod
     def set_pixel_color(ensemble, pixel, r, g, b):
-        ensemble_len = len(ensemble)
-
-        if ensemble_len != 2:
-            raise ValueError("Ensemble must contain exactly two frames_library!")
-
-        for frame in ensemble:
-            num_values = len(frame)
-
-            if num_values != 48:
-                raise ValueError("All frames_library must have exactly 48 values!")
-
-            is_numeric = True
-            is_positive = True
-            is_small = True
-
-            for value in frame:
-                is_numeric = is_numeric and isinstance(value, int)
-                is_positive = is_positive and not (value < 0)
-                is_small = is_small and value < 256
-
-                if not is_numeric:
-                    raise TypeError("All frame values must be integers!")
-                if not is_positive:
-                    raise ValueError("All frame values must be positive!")
-                if not is_small:
-                    raise ValueError("All frame values must be smaller than 256!")
-
-        if not isinstance(pixel, int):
-            raise TypeError("Pixel must be an integer!")
-        if not (0 < pixel < 33):
-            raise ValueError("Pixel must be in the range of 1 to 32!")
-        if not (isinstance(r, int) and isinstance(g, int) and isinstance(b, int)):
-            raise TypeError("Color values must be integers!")
-        if not ((0 < r < 256) and (0 < g < 256) and (0 < b < 256)):
-            raise ValueError("Color values must be in the range of 1 to 255!")
+        if not 0 <= pixel <= 31:
+            raise ValueError("Pixel index must be an integer between 0 and 31!")
 
         if pixel < 16:
-            index = 0
+            ensemble.l_frame.adjust_color(pixel, r, g, b)
         else:
-            index = 1
             pixel = pixel - 16
-
-        ensemble[index][pixel + 0] = r
-        ensemble[index][pixel + 1] = g
-        ensemble[index][pixel + 2] = b
+            ensemble.r_frame.adjust_color(pixel, r, g, b)
 
         return ensemble

src/visualising/visualiser.py

@@ -3,6 +3,7 @@
 import rospy
 
 from visualising.monitoring.watchdog import Watchdog
+from visualising.expression.expression import Emotion
 from visualising.communication.arduino import Arduino
 from visualising.communication.channel.connection import Connection
 
@@ -13,11 +14,11 @@ class Visualiser:
         baud = rospy.get_param("/visualiser/arduino_baud")
         freq = rospy.get_param("/visualiser/vis_freq")
 
-        # self.arduino = Arduino(Connection(port, baud))
+        self.arduino = Arduino(Connection(port, baud))
         self.watchdog = Watchdog()
 
         if rospy.get_param("/visualiser/visualisation_strategy") == 1:
-            self.expression = None
+            self.expression = Emotion(self.arduino)
         else:
             self.expression = None
 
@@ -30,5 +31,5 @@ class Visualiser:
         self.timer = rospy.Timer(rospy.Duration(1.0 / freq), self.visualise)
 
     def visualise(self, event):
-        # self.expression.react()
-        rospy.logwarn(self.watchdog.aggregated_domains)
+        state = [self.watchdog.aggregated_domains, self.watchdog.aggregated_metrics]
+        self.expression.react(state)