diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdd3380862f195decb1c8dd2ef116d189b5b3b93..e4a86b7aeefa13cae104b3115028e4dbdd585853 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 0000000000000000000000000000000000000000..0d2c235a8af317a531ffa9be244984e49e194967
--- /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 0000000000000000000000000000000000000000..d6705831806ee8d3908995353186638fe8aa46c5
--- /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 0000000000000000000000000000000000000000..d202ccc2f718602bcb2b6c3a6afc56fe8ddaee8b
--- /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 ae086d42738c966245e0bbfca0bf685297156a76..07e0e91f434ec7cd5ed1b2b6da08010b87a9c312 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 162c2982d6abc6a0a426619433a9d082dc485321..a639dfd8e14a3aa679d3948899d4733fea68d776 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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/visualising/animations/animation_happy.txt b/src/visualising/animations/animation_happy.txt
new file mode 100644
index 0000000000000000000000000000000000000000..661bc2bc745418543262e99a54b7601abf8aa080
--- /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 0000000000000000000000000000000000000000..cfbcd9a63da0bb5b555083656b81b0ba411b6004
--- /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 0000000000000000000000000000000000000000..b39214e1c226433a3a70c2f7397e1dd814c3220e
--- /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 2ed3d5d7bec57d72948fa0fe01fdfb1d071b7cc1..896deadf5c665091ca087257be35ce25798318e2 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 7d861ed3e370c48accffa941983ab4f3d0c4175a..745c536dacb5430ccd50bb344aaa0d458b68ba49 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 82abeb9b58619c848f5a1d6bc3e3ff01a6af33b3..0671296631cea33ac1ea6726685f5eb0d7d77bdb 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 0000000000000000000000000000000000000000..d5123ca9b82fccb3a21de42790376812988814f5
--- /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)