From 33a15e65988d0f37bcac8df9238526f3784b7fbf Mon Sep 17 00:00:00 2001
From: bav6096 <benedikt.deike@informatik.uni-hamburg.de>
Date: Mon, 1 Nov 2021 23:39:13 +0100
Subject: [PATCH] implemented basic visualiser

---
 CMakeLists.txt                                |  6 +--
 msg/State.msg                                 |  3 ++
 msg/States.msg                                |  1 +
 nodes/visualiser                              | 20 ++++++++
 setup.py                                      |  4 +-
 src/visualising/animation.py                  |  9 +++-
 src/visualising/animations/__init__.py        |  0
 .../animations/animation_happy.txt            |  4 ++
 src/visualising/animations/animation_ok.txt   |  4 ++
 src/visualising/animations/animation_sad.txt  |  4 ++
 src/visualising/arduino.py                    |  7 +--
 src/visualising/connection.py                 |  3 +-
 src/visualising/exception.py                  |  1 +
 src/visualising/visualiser.py                 | 49 +++++++++++++++++++
 14 files changed, 104 insertions(+), 11 deletions(-)
 create mode 100644 msg/State.msg
 create mode 100644 msg/States.msg
 create mode 100755 nodes/visualiser
 create mode 100644 src/visualising/animations/__init__.py
 create mode 100644 src/visualising/animations/animation_happy.txt
 create mode 100644 src/visualising/animations/animation_ok.txt
 create mode 100644 src/visualising/animations/animation_sad.txt
 create mode 100644 src/visualising/visualiser.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdd3380..e4a86b7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,8 +49,8 @@ catkin_python_setup()
 ## Generate messages in the 'msg' folder
 add_message_files(
    FILES
-#   Message1.msg
-#   Message2.msg
+   State.msg
+   States.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -161,7 +161,7 @@ include_directories(
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-#   scripts/my_python_script
+   nodes/visualiser
    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
diff --git a/msg/State.msg b/msg/State.msg
new file mode 100644
index 0000000..0d2c235
--- /dev/null
+++ b/msg/State.msg
@@ -0,0 +1,3 @@
+string  component # Name of the component, whose
+                  # critical level is monitored.
+float32 critical  # Critical level of the family.
\ No newline at end of file
diff --git a/msg/States.msg b/msg/States.msg
new file mode 100644
index 0000000..d670583
--- /dev/null
+++ b/msg/States.msg
@@ -0,0 +1 @@
+State[] states # Array of states.
diff --git a/nodes/visualiser b/nodes/visualiser
new file mode 100755
index 0000000..d202ccc
--- /dev/null
+++ b/nodes/visualiser
@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+
+import rospy
+from visualising.visualiser import Visualiser
+
+
+def visualiser():
+    rospy.init_node("visualiser")
+    node = Visualiser()
+
+    rate = rospy.Rate(10)
+    while not rospy.is_shutdown():
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        visualiser()
+    except rospy.ROSInterruptException:
+        pass
diff --git a/setup.py b/setup.py
index ae086d4..07e0e91 100644
--- a/setup.py
+++ b/setup.py
@@ -4,8 +4,8 @@ from distutils.core import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
 setup_args = generate_distutils_setup(
-    packages=['visualising'],
-    package_dir={'': 'src'}
+    packages=["visualising"],
+    package_dir={"": "src"}
 )
 
 setup(**setup_args)
diff --git a/src/visualising/animation.py b/src/visualising/animation.py
index 162c298..a639dfd 100755
--- a/src/visualising/animation.py
+++ b/src/visualising/animation.py
@@ -1,6 +1,9 @@
 #!/usr/bin/env python
+
 import re
-from .exception import AnimationException
+import importlib.resources
+from . import animations
+from visualising.exception import AnimationException
 
 
 class Animation:
@@ -66,7 +69,7 @@ class Animation:
     @staticmethod
     def read_frames_from_file(filename):
         try:
-            file = open(filename, 'r')
+            file = importlib.resources.open_text(animations, filename)
         except AttributeError:
             raise AnimationException("P: -- ERROR -- The file is incorrectly formatted!")
         except FileNotFoundError:
@@ -77,8 +80,10 @@ class Animation:
         animation = []
         for line in file:
             frame = []
+
             for value in line.split(','):
                 frame.append(int(re.search(r'\d+', value).group()))
+
             animation.append(frame)
 
         return animation
diff --git a/src/visualising/animations/__init__.py b/src/visualising/animations/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/visualising/animations/animation_happy.txt b/src/visualising/animations/animation_happy.txt
new file mode 100644
index 0000000..661bc2b
--- /dev/null
+++ b/src/visualising/animations/animation_happy.txt
@@ -0,0 +1,4 @@
+0,0,50,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,50,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
\ No newline at end of file
diff --git a/src/visualising/animations/animation_ok.txt b/src/visualising/animations/animation_ok.txt
new file mode 100644
index 0000000..cfbcd9a
--- /dev/null
+++ b/src/visualising/animations/animation_ok.txt
@@ -0,0 +1,4 @@
+0,50,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,50,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
\ No newline at end of file
diff --git a/src/visualising/animations/animation_sad.txt b/src/visualising/animations/animation_sad.txt
new file mode 100644
index 0000000..b39214e
--- /dev/null
+++ b/src/visualising/animations/animation_sad.txt
@@ -0,0 +1,4 @@
+50,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+50,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
\ No newline at end of file
diff --git a/src/visualising/arduino.py b/src/visualising/arduino.py
index 2ed3d5d..896dead 100755
--- a/src/visualising/arduino.py
+++ b/src/visualising/arduino.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python
+
 import time
-from . message import FrameMsg, InstrMsg
-from . animation import Animation
-from . exception import ArduinoException
+from visualising.message import FrameMsg, InstrMsg
+from visualising.animation import Animation
+from visualising.exception import ArduinoException
 
 
 class Arduino:
diff --git a/src/visualising/connection.py b/src/visualising/connection.py
index 7d861ed..745c536 100755
--- a/src/visualising/connection.py
+++ b/src/visualising/connection.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python
+
 import time
 import serial
 
-from . exception import ConnectionException
+from visualising.exception import ConnectionException
 from serial.serialutil import SerialException
 
 
diff --git a/src/visualising/exception.py b/src/visualising/exception.py
index 82abeb9..0671296 100755
--- a/src/visualising/exception.py
+++ b/src/visualising/exception.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+
 class Error(Exception):
     pass
 
diff --git a/src/visualising/visualiser.py b/src/visualising/visualiser.py
new file mode 100644
index 0000000..d5123ca
--- /dev/null
+++ b/src/visualising/visualiser.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+
+import rospy
+from visualising.msg import State
+from visualising.msg import States
+from visualising.arduino import Arduino
+from visualising.connection import Connection
+from visualising.animation import Animation
+from visualising.exception import *
+
+
+class Visualiser:
+    def __init__(self, port="/dev/ttyUSB0", baud=57600):
+        self.arduino = Arduino(Connection(port, baud))
+        self.robot_state = 0  # Robot starts in a happy state.
+
+        self.happy = Animation(Animation.read_frames_from_file("animation_happy.txt"), 100, 1)
+        self.ok = Animation(Animation.read_frames_from_file("animation_ok.txt"), 100, 1)
+        self.sad = Animation(Animation.read_frames_from_file("animation_sad.txt"), 100, 1)
+
+        rospy.Subscriber("states", States, self.evaluate)
+
+        animation_duration = 1
+        self.timer = rospy.Timer(rospy.Duration(animation_duration), self.update_mood)
+
+    # TODO: Add different modes!
+    def evaluate(self, event):
+        number = 0
+        value = 0
+
+        for state in event.states:
+            value = value + state.critical
+            number = number + 1
+
+        self.robot_state = value / number
+
+    # TODO: Add different modes!
+    def update_mood(self, event):
+        print(self.robot_state)
+        if self.robot_state > 0.7:
+            self.stream_animation(self.sad)
+        else:
+            if self.robot_state > 0.4:
+                self.stream_animation(self.ok)
+            else:
+                self.stream_animation(self.happy)
+
+    def stream_animation(self, animation):
+        self.arduino.stream_animation(animation)
-- 
GitLab