Implement the first draft of the visualizer

Author: GiulioRomualdi

app.py

@@ -0,0 +1,55 @@
+# sys
+import sys
+import os
+
+# GUI
+from ui.gui import RobotViewerMainWindow
+from PyQt5.QtWidgets import QApplication
+
+from file_reader.signal_provider import SignalProvider
+
+# Meshcat
+from idyntree.visualize import MeshcatVisualizer
+
+
+def get_model_path(robot_name='iCubGazeboV3'):
+    seperbuild_prefix = os.environ["ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX"]
+    robots_folder = os.path.join("share", "iCub", "robots", robot_name)
+
+    return os.path.join(seperbuild_prefix, robots_folder, "model.urdf")
+
+
+def get_joint_list():
+    joint_list = ['neck_pitch', 'neck_roll', 'neck_yaw',
+                  'torso_pitch', 'torso_roll', 'torso_yaw',
+                  'l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow',
+                  'r_shoulder_pitch', 'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow',
+                  'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll',
+                  'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll']
+
+    return joint_list
+
+
+if __name__ == '__main__':
+    # instantiate device_manager
+    meshcat = MeshcatVisualizer()
+    meshcat.set_model_from_file(get_model_path(), get_joint_list())
+    meshcat.load_model(color=[1, 1, 1, 0.8])
+
+    signal_provider = SignalProvider(meshcat)
+
+    # instantiate a QApplication
+    app = QApplication(sys.argv)
+
+    # instantiate the main window
+    gui = RobotViewerMainWindow(meshcat=meshcat, signal_provider=signal_provider)
+
+    # show the main window
+    gui.show()
+
+    signal_provider.start()
+
+    sys.exit(app.exec_())
+
+
+    

file_reader/__init__.py

@@ -0,0 +1,83 @@
+import time
+
+import h5py
+import numpy as np
+from PyQt5.QtCore import pyqtSignal, QThread
+from threading import Lock
+
+
+class SignalProvider(QThread):
+    update_index = pyqtSignal()
+
+    def __init__(self, meshcat_visualizer):
+        QThread.__init__(self)
+
+        # set device state
+        self._state = 'pause'
+        self.state_lock = Lock()
+
+        self._last_data = None
+
+        self._index = 0
+        self.index_lock = Lock()
+
+        self.fps = 50
+        self.meshcat_visualizer = meshcat_visualizer
+
+        self.s = np.array([])
+
+    def open_mat_file(self, file_name: str):
+        with h5py.File(file_name, 'r') as f:
+            self.s = np.squeeze(np.array(f['robot_logger_device']['joints_state']['positions']['data']))
+            self.index = 0
+
+    def __len__(self):
+        return self.s.shape[0]
+
+    @property
+    def state(self):
+        self.state_lock.acquire()
+        value = self._state
+        self.state_lock.release()
+        return value
+
+    @state.setter
+    def state(self, new_state):
+        self.state_lock.acquire()
+        self._state = new_state
+        self.state_lock.release()
+
+    @property
+    def index(self):
+        self.index_lock.acquire()
+        value = self._index
+        self.index_lock.release()
+        return value
+
+    @index.setter
+    def index(self, index):
+        self.index_lock.acquire()
+        self._index = index
+        self.index_lock.release()
+
+    def register_update_index(self, slot):
+        self.update_index.connect(slot)
+
+    def run(self):
+        while True:
+            if self.state == 'running':
+                temp_index = min(self.index, self.s.shape[0] - 1)
+                self._last_data = self.s[temp_index, :]
+                self.index = temp_index + int(100/self.fps)
+                R = np.eye(3)
+                p = np.array([0.0, 0.0, 0.0])
+                self.meshcat_visualizer.display(p, R, self._last_data)
+                self.update_index.emit()
+
+            time.sleep(1/self.fps)
+
+
+
+
+
+

file_reader/signal_provider.py

@@ -0,0 +1,3 @@
+#! /bin/bash
+echo "Generate UI"
+pyuic5 -o visualizer.py visualizer.ui

ui/__init__.py

@@ -0,0 +1,157 @@
+#PyQt5
+from PyQt5 import QtWidgets
+from PyQt5.QtCore import QUrl
+from PyQt5.QtCore import pyqtSlot
+from PyQt5.QtWidgets import QFileDialog
+
+import sys
+
+# QtDesigner generated classes
+from ui.visualizer import Ui_MainWindow
+
+# for logging
+from time import localtime, strftime
+
+
+class RobotViewerMainWindow(QtWidgets.QMainWindow):
+    """
+    Main window class of EVB1000 Viewer
+    """
+    def __init__(self, meshcat: str, signal_provider):
+        # call QMainWindow constructor
+        super().__init__()
+
+        # set up the user interface
+        self.ui = Ui_MainWindow()
+        self.ui.setupUi(self)
+
+        self.signal_provider = signal_provider
+        self.signal_size = len(self.signal_provider)
+        self.signal_provider.register_update_index(self.update_slider)
+
+        # instantiate the Logger
+        self.logger = Logger(self.ui.logLabel, self.ui.logScrollArea)
+
+        self.slider_pressed = False
+
+        # connect action
+        self.ui.actionQuit.triggered.connect(self.quit)
+        self.ui.actionOpen.triggered.connect(self.open_mat_file)
+
+        self.ui.meshcatView.setUrl(QUrl(meshcat.viewer.url()))
+
+        self.ui.pauseButton.clicked.connect(self.pauseButton_on_click)
+        self.ui.startButton.clicked.connect(self.startButton_on_click)
+        self.ui.timeSlider.sliderReleased.connect(self.timeSlider_on_release)
+        self.ui.timeSlider.sliderPressed.connect(self.timeSlider_on_pressed)
+
+    def timeSlider_on_pressed(self):
+        self.slider_pressed = True
+
+    def timeSlider_on_release(self):
+        index = int(self.ui.timeSlider.value() / 100 * self.signal_size)
+        self.signal_provider.index = index
+        self.slider_pressed = False
+        self.logger.write_to_log("Dataset index set at " + str(index) + ".")
+
+
+    def startButton_on_click(self):
+        self.ui.startButton.setEnabled(False)
+        self.ui.pauseButton.setEnabled(True)
+        self.signal_provider.state = "running"
+
+        self.logger.write_to_log("Dataset started.")
+
+    def pauseButton_on_click(self):
+        self.ui.pauseButton.setEnabled(False)
+        self.ui.startButton.setEnabled(True)
+        self.signal_provider.state = "pause"
+
+        self.logger.write_to_log("Dataset paused.")
+
+    @pyqtSlot()
+    def update_slider(self):
+        if not self.slider_pressed:
+            index = 100 * self.signal_provider.index / self.signal_size
+            self.ui.timeSlider.setValue(index)
+
+    def quit(self):
+        """
+        Quit method.
+
+        Method called when actionQuit is triggered.
+        """
+
+        # close the window
+        self.close()
+
+    def open_mat_file(self):
+        file_name, _ = QFileDialog.getOpenFileName(self, "Open a mat file", ".", filter='*.mat')
+        self.signal_provider.open_mat_file(file_name)
+        self.ui.startButton.setEnabled(True)
+        self.ui.timeSlider.setEnabled(True)
+        self.ui.timeLabel.setEnabled(True)
+        self.signal_size = len(self.signal_provider)
+        self.logger.write_to_log("File '" + file_name + "' opened.")
+
+
+class Logger:
+    """
+    Logger class shows events during the execution of the viewer.
+    """
+    
+    def __init__(self, log_widget, scroll_area):
+
+        # set log widget from main window
+        self.log_widget = log_widget
+
+        # set scroll area form main window
+        self.scroll_area = scroll_area
+
+        # print welcome message
+        self.write_to_log("Robot Viewer started.")
+
+    def write_to_log(self, text):
+        """
+        Log the text "text" with a timestamp.
+        """
+
+        # extract current text from the log widget
+        current_text = self.log_widget.text()
+
+        # compose new text
+        # convert local time to string
+        time_str = strftime(" [%H:%M:%S] ", localtime())
+        # 
+        new_text = current_text + time_str + text + "\n"
+
+        # log into the widget
+        self.log_widget.setText(new_text)
+
+        # scroll down text
+        self.scroll_down()
+
+    def scroll_down(self):
+        """
+        Scroll down the slider of the scroll area
+        linked to this logger
+        """
+        # extract scroll bar from the scroll area
+        scroll_bar = self.scroll_area.verticalScrollBar()
+
+        # set maximum value of the slider, 1000 is enough
+        scroll_bar.setMaximum(1000)
+        scroll_bar.setValue(scroll_bar.maximum())
+
+
+if __name__ == '__main__':
+    # construct a QApplication
+    app = QtWidgets.QApplication(sys.argv)
+
+    # instantiate the main window and add the Matplotlib canvas
+    gui = RobotViewerMainWindow()
+
+    # show the main window
+    gui.show()
+    
+    sys.exit(app.exec_())