diff --git a/robot_log_visualizer/file_reader/signal_provider.py b/robot_log_visualizer/file_reader/signal_provider.py index 6d92ea2..2036864 100644 --- a/robot_log_visualizer/file_reader/signal_provider.py +++ b/robot_log_visualizer/file_reader/signal_provider.py @@ -2,6 +2,7 @@ # This software may be modified and distributed under the terms of the # Released under the terms of the BSD 3-Clause License +import sys import time import math import h5py @@ -10,6 +11,12 @@ from robot_log_visualizer.utils.utils import PeriodicThreadState, RobotStatePath import idyntree.swig as idyn +import bipedal_locomotion_framework.bindings as blf + +# for real-time logging +import yarp +import json + class TextLoggingMsg: def __init__(self, level, text): @@ -67,7 +74,17 @@ def __init__(self, period: float): self._current_time = 0 + self.realtimeBufferReached = False + self.initMetadata = False + self.realtimeFixedPlotWindow = 20 + + # for networking with the real-time logger + self.realtimeNetworkInit = False + self.vectorCollectionsClient = blf.yarp_utilities.VectorsCollectionClient() self.trajectory_span = 200 + self.rtMetadataDict = {} + self.updateMetadataVal = 0 + self.updateMetadata = False def __populate_text_logging_data(self, file_object): data = {} @@ -121,8 +138,10 @@ def __populate_numerical_data(self, file_object): if not isinstance(value, h5py._hl.group.Group): continue if key == "#refs#": + print("Skipping for refs") continue if key == "log": + print("Skipping for log") continue if "data" in value.keys(): data[key] = {} @@ -145,13 +164,121 @@ def __populate_numerical_data(self, file_object): "".join(chr(c[0]) for c in value[ref]) for ref in elements_names_ref[0] ] + else: data[key] = self.__populate_numerical_data(file_object=value) return data + def __populateRealtimeLoggerData(self, rawData, keys, value, recentTimestamp): + if keys[0] not in rawData: + rawData[keys[0]] = {} + + if len(keys) == 1: + rawData[keys[0]]["data"] = np.append(rawData[keys[0]]["data"], value).reshape(-1, len(value)) + rawData[keys[0]]["timestamps"] = np.append(rawData[keys[0]]["timestamps"], recentTimestamp) + + tempInitialTime = rawData[keys[0]]["timestamps"][0] + tempEndTime = rawData[keys[0]]["timestamps"][-1] + while tempEndTime - tempInitialTime > self.realtimeFixedPlotWindow: + rawData[keys[0]]["data"] = np.delete(rawData[keys[0]]["data"], 0, axis=0) + rawData[keys[0]]["timestamps"] = np.delete(rawData[keys[0]]["timestamps"], 0) + tempInitialTime = rawData[keys[0]]["timestamps"][0] + tempEndTime = rawData[keys[0]]["timestamps"][-1] + + else: + self.__populateRealtimeLoggerData(rawData[keys[0]], keys[1:], value, recentTimestamp) + + def __populateRealtimeLoggerMetadata(self, rawData, keys, value): + if keys[0] == "timestamps": + return + if keys[0] not in rawData: + rawData[keys[0]] = {} + + if len(keys) == 1: + if len(value) == 0: + del rawData[keys[0]] + return + if "elements_names" not in rawData[keys[0]]: + rawData[keys[0]]["elements_names"] = np.array([]) + rawData[keys[0]]["data"] = np.array([]) + rawData[keys[0]]["timestamps"] = np.array([]) + + rawData[keys[0]]["elements_names"] = np.append(rawData[keys[0]]["elements_names"], value) + else: + self.__populateRealtimeLoggerMetadata(rawData[keys[0]], keys[1:], value) + + + def establish_connection(self): + if not self.realtimeNetworkInit: + yarp.Network.init() + + param_handler = blf.parameters_handler.YarpParametersHandler() + param_handler.set_parameter_string("remote", "/rtLoggingVectorCollections") # you must have some local port as well + param_handler.set_parameter_string("local", "/visualizerInput") # remote must match the server + param_handler.set_parameter_string("carrier", "udp") + self.vectorCollectionsClient.initialize(param_handler) + + self.vectorCollectionsClient.connect() + self.realtimeNetworkInit = True + self.rtMetadataDict = self.vectorCollectionsClient.get_metadata().getVectors() + if not self.rtMetadataDict: + print("Failed to read realtime YARP port, closing") + return False + + self.joints_name = self.rtMetadataDict["robot_realtime::description_list"] + self.robot_name = self.rtMetadataDict["robot_realtime::yarp_robot_name"][0] + for keyString, value in self.rtMetadataDict.items(): + keys = keyString.split("::") + self.__populateRealtimeLoggerMetadata(self.data, keys, value) + del self.data["robot_realtime"]["description_list"] + del self.data["robot_realtime"]["yarp_robot_name"] + del self.data["robot_realtime"]["newMetadata"] + + input = self.vectorCollectionsClient.read_data(True) + + if not input: + print("Failed to read realtime YARP port, closing") + return False + else: + # Update the timestamps + recentTimestamp = input["robot_realtime::timestamps"][0] + self.timestamps = np.append(self.timestamps, recentTimestamp).reshape(-1) + del input["robot_realtime::timestamps"] + + # Keep the data within the fixed time interval + while recentTimestamp - self.timestamps[0] > self.realtimeFixedPlotWindow: + self.initial_time = self.timestamps[0] + self.end_time = self.timestamps[-1] + self.timestamps = np.delete(self.timestamps, 0).reshape(-1) + self.initial_time = self.timestamps[0] + self.end_time = self.timestamps[-1] + + # Check if new metadata arrived for a new signal + newMetadataInputVal = input["robot_realtime::newMetadata"][0] + self.updateMetadata = newMetadataInputVal != self.updateMetadataVal + del input["robot_realtime::newMetadata"] + + # If there is new metadata, populate it + if self.updateMetadata: + self.updateMetadataVal = newMetadataInputVal + metadata = self.vectorCollectionsClient.get_metadata().getVectors() + difference = { k : metadata[k] for k in set(metadata) - set(self.rtMetadataDict) } + self.rtMetadataDict = metadata + for keyString, value in difference.items(): + keys = keyString.split("::") + self.__populateRealtimeLoggerMetadata(self.data, keys, value) + + # Store the new data that comes in + for keyString, value in input.items(): + keys = keyString.split("::") + self.__populateRealtimeLoggerData(self.data, keys, value, recentTimestamp) + + return True + def open_mat_file(self, file_name: str): with h5py.File(file_name, "r") as file: + root_variable = file.get(self.root_name) self.data = self.__populate_numerical_data(file) diff --git a/robot_log_visualizer/plotter/matplotlib_viewer_canvas.py b/robot_log_visualizer/plotter/matplotlib_viewer_canvas.py index 92a385a..d804a5e 100644 --- a/robot_log_visualizer/plotter/matplotlib_viewer_canvas.py +++ b/robot_log_visualizer/plotter/matplotlib_viewer_canvas.py @@ -3,6 +3,7 @@ # Released under the terms of the BSD 3-Clause License # PyQt +import numpy as np from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar from matplotlib.figure import Figure @@ -64,27 +65,28 @@ def quit_animation(self): if self.vertical_line_anim: self.vertical_line_anim._stop() - def update_plots(self, paths, legends): + def update_plots(self, paths, legends, realtimePlot=False): + self.axes.cla() for path, legend in zip(paths, legends): path_string = "/".join(path) legend_string = "/".join(legend[1:]) - if path_string not in self.active_paths.keys(): - data = self.signal_provider.data - for key in path[:-1]: - data = data[key] - try: - datapoints = data["data"][:, int(path[-1])] - except IndexError: - # This happens in the case the variable is a scalar. - datapoints = data["data"][:] - - timestamps = data["timestamps"] - self.signal_provider.initial_time - - (self.active_paths[path_string],) = self.axes.plot( - timestamps, datapoints, label=legend_string - ) - + #if path_string not in self.active_paths.keys(): + data = self.signal_provider.data.copy() + for key in path[:-1]: + data = data[key] + try: + datapoints = data["data"][:, int(path[-1])] + except IndexError: + # This happens in the case the variable is a scalar. + datapoints = data["data"][:] + + timestamps = data["timestamps"] - self.signal_provider.initial_time + + (self.active_paths[path_string],) = self.axes.plot( + timestamps, datapoints, label=legend_string + ) + self.axes.grid() paths_to_be_canceled = [] for active_path in self.active_paths.keys(): path = active_path.split("/") @@ -96,9 +98,13 @@ def update_plots(self, paths, legends): self.active_paths[path].remove() self.active_paths.pop(path) - self.axes.set_xlim( - 0, self.signal_provider.end_time - self.signal_provider.initial_time - ) + if realtimePlot: + #self.axes.autoscale() + self.axes.set_xlim(0, self.signal_provider.realtimeFixedPlotWindow) + else: + self.axes.set_xlim( + 0, self.signal_provider.end_time - self.signal_provider.initial_time + ) # Since a new plot has been added/removed we delete the old animation and we create a new one # TODO: this part could be optimized @@ -127,4 +133,4 @@ def update_vertical_line(self, _): # Draw vertical line at current index self.vertical_line.set_data([current_time, current_time], self.axes.get_ylim()) - return self.vertical_line, *(self.active_paths.values()) + return self.vertical_line, *(self.active_paths.values()) \ No newline at end of file diff --git a/robot_log_visualizer/robot_visualizer/meshcat_provider.py b/robot_log_visualizer/robot_visualizer/meshcat_provider.py index 3288943..42592b4 100644 --- a/robot_log_visualizer/robot_visualizer/meshcat_provider.py +++ b/robot_log_visualizer/robot_visualizer/meshcat_provider.py @@ -37,6 +37,8 @@ def __init__(self, signal_provider, period): self._registered_3d_points = set() self._registered_3d_trajectories = dict() + self._realtimeMeshUpdate = False + @property def state(self): locker = QMutexLocker(self.state_lock) @@ -175,6 +177,7 @@ def run(self): index = self._signal_provider.index if self.state == PeriodicThreadState.running and self._is_model_loaded: robot_state = self._signal_provider.get_robot_state_at_index(index) + self.meshcat_visualizer_mutex.lock() # These are the robot measured joint positions in radians self._meshcat_visualizer.set_multibody_system_state( @@ -224,3 +227,15 @@ def run(self): if self.state == PeriodicThreadState.closed: return + + # For the real-time logger + def updateMeshRealtime(self): + self._signal_provider.index = len(self._signal_provider.timestamps) - 1 + robot_state = self._signal_provider.get_robot_state_at_index(self._signal_provider.index) + + self._meshcat_visualizer.set_multibody_system_state( + base_position=robot_state["base_position"], + base_rotation=robot_state["base_orientation"], + joint_value=robot_state["joints_position"][self.model_joints_index], + model_name="robot", + ) diff --git a/robot_log_visualizer/ui/autogenerated/visualizer.py b/robot_log_visualizer/ui/autogenerated/visualizer.py index 7e3c9be..44519f6 100644 --- a/robot_log_visualizer/ui/autogenerated/visualizer.py +++ b/robot_log_visualizer/ui/autogenerated/visualizer.py @@ -239,15 +239,24 @@ def setupUi(self, MainWindow): icon = QtGui.QIcon.fromTheme("exit") self.actionQuit.setIcon(icon) self.actionQuit.setObjectName("actionQuit") + + # Add the GUI components for the open action self.actionOpen = QtWidgets.QAction(MainWindow) icon = QtGui.QIcon.fromTheme("document-open") self.actionOpen.setIcon(icon) self.actionOpen.setObjectName("actionOpen") + + # Add a GUI action for connecting to the YARP port + # for real-time logging + self.actionConnect = QtWidgets.QAction(MainWindow) + self.actionConnect.setObjectName("actionConnect") + self.actionAbout = QtWidgets.QAction(MainWindow) self.actionAbout.setObjectName("actionAbout") self.actionSet_Robot_Model = QtWidgets.QAction(MainWindow) self.actionSet_Robot_Model.setObjectName("actionSet_Robot_Model") self.menuFile.addAction(self.actionOpen) + self.menuFile.addAction(self.actionConnect) self.menuFile.addSeparator() self.menuFile.addAction(self.actionQuit) self.menuHelp.addAction(self.actionAbout) @@ -275,6 +284,7 @@ def retranslateUi(self, MainWindow): self.actionQuit.setText(_translate("MainWindow", "&Quit")) self.actionQuit.setShortcut(_translate("MainWindow", "Ctrl+Q")) self.actionOpen.setText(_translate("MainWindow", "&Open")) + self.actionConnect.setText(_translate("MainWindow", "Realtime Connect")) self.actionOpen.setShortcut(_translate("MainWindow", "Ctrl+O")) self.actionAbout.setText(_translate("MainWindow", "About")) self.actionSet_Robot_Model.setText(_translate("MainWindow", "Set Robot Model")) diff --git a/robot_log_visualizer/ui/gui.py b/robot_log_visualizer/ui/gui.py index e3428d4..b204334 100644 --- a/robot_log_visualizer/ui/gui.py +++ b/robot_log_visualizer/ui/gui.py @@ -3,9 +3,10 @@ # Released under the terms of the BSD 3-Clause License # PyQt5 +import threading from PyQt5 import QtWidgets, QtGui from PyQt5.QtCore import QUrl -from PyQt5.QtCore import pyqtSlot, Qt, QMutex, QMutexLocker +from PyQt5.QtCore import pyqtSlot, Qt, QMutex, QMutexLocker, QThread from PyQt5.QtWidgets import ( QFileDialog, QTreeWidgetItem, @@ -48,6 +49,10 @@ from pyqtconsole.console import PythonConsole import pyqtconsole.highlighter as hl +import time + +import yarp + class SetRobotModelDialog(QtWidgets.QDialog): def __init__( @@ -70,6 +75,7 @@ def __init__( self.ui.robotModelToolButton.clicked.connect(self.open_urdf_file) self.ui.packageDirToolButton.clicked.connect(self.open_package_directory) + def open_urdf_file(self): file_name, _ = QFileDialog.getOpenFileName( self, "Open urdf file", ".", filter="*.urdf" @@ -120,12 +126,19 @@ def get_icon(icon_name): ) return icon - class RobotViewerMainWindow(QtWidgets.QMainWindow): def __init__(self, signal_provider, meshcat_provider, animation_period): # call QMainWindow constructor super().__init__() + # for realtime logging + self.realtimePlotUpdaterThreadActive = False + self.plotData = {} + self.plottingLock = threading.Lock() + self.realtimeConnectionEnabled = False + self.timeoutAttempts = 20 + self.sleepPeriodBuffer = 0.02 + self.animation_period = animation_period # set up the user interface @@ -145,6 +158,7 @@ def __init__(self, signal_provider, meshcat_provider, animation_period): self.ui.actionQuit.setIcon(get_icon("close-circle-outline.svg")) self.ui.actionQuit.setIcon(get_icon("close-circle-outline.svg")) + self.ui.actionConnect.setIcon(get_icon("connection-outline.png")) self.ui.actionOpen.setIcon(get_icon("folder-open-outline.svg")) self.ui.actionSet_Robot_Model.setIcon(get_icon("body-outline.svg")) self.setWindowIcon(get_icon("icon.png")) @@ -185,6 +199,7 @@ def __init__(self, signal_provider, meshcat_provider, animation_period): # connect action self.ui.actionQuit.triggered.connect(self.close) self.ui.actionOpen.triggered.connect(self.open_mat_file) + self.ui.actionConnect.triggered.connect(self.connect_realtime_logger) self.ui.actionAbout.triggered.connect(self.open_about) self.ui.actionSet_Robot_Model.triggered.connect(self.open_set_robot_model) @@ -239,6 +254,7 @@ def __init__(self, signal_provider, meshcat_provider, animation_period): self.ui.pythonWidgetLayout.addWidget(self.pyconsole) self.pyconsole.eval_in_thread() + # self.media_player = QMediaPlayer(None, QMediaPlayer.VideoSurface) # self.media_player.setVideoOutput(self.ui.webcamView) # self.media_loaded = False @@ -302,7 +318,6 @@ def keyPressEvent(self, event): self.ui.timeSlider.setValue(new_index) self.slider_pressed = False - def toolButton_on_click(self): self.plot_items.append( PlotItem(signal_provider=self.signal_provider, period=self.animation_period) @@ -354,7 +369,7 @@ def startButton_on_click(self): self.ui.startButton.setEnabled(False) self.ui.pauseButton.setEnabled(True) self.signal_provider.state = PeriodicThreadState.running - # self.meshcat_provider.state = PeriodicThreadState.running + self.meshcat_provider.state = PeriodicThreadState.running self.logger.write_to_log("Dataset started.") @@ -367,17 +382,25 @@ def pauseButton_on_click(self): video_item.media_player.pause() self.signal_provider.state = PeriodicThreadState.pause - # self.meshcat_provider.state = PeriodicThreadState.pause + self.meshcat_provider.state = PeriodicThreadState.pause self.logger.write_to_log("Dataset paused.") def plotTabCloseButton_on_click(self, index): + self.plottingLock.acquire() self.ui.tabPlotWidget.removeTab(index) self.plot_items[index].canvas.quit_animation() + + # Update the indexes of plotData before deletion + for i in range(index, len(self.plotData.keys()) - 1): + self.plotData[i] = self.plotData[i + 1] + # Remove the last key + del self.plotData[list(self.plotData.keys())[-1]] del self.plot_items[index] if self.ui.tabPlotWidget.count() == 1: self.ui.tabPlotWidget.setTabsClosable(False) + self.plottingLock.release() def plotTabBar_on_doubleClick(self, index): dlg, plot_title = build_plot_title_box_dialog() @@ -410,9 +433,13 @@ def variableTreeWidget_on_click(self): if not paths: return + self.plottingLock.acquire() + self.plotData[self.ui.tabPlotWidget.currentIndex()] = {"paths": paths, "legends": legends} + self.plot_items[self.ui.tabPlotWidget.currentIndex()].canvas.update_plots( - paths, legends + paths, legends, self.realtimeConnectionEnabled ) + self.plottingLock.release() def find_text_log_index(self, path): current_time = self.signal_provider.current_time @@ -528,6 +555,9 @@ def closeEvent(self, event): self.signal_provider.wait() event.accept() + if self.realtimeConnectionEnabled: + self.realtimeConnectionEnabled = False + self.networkThread.join() def __populate_variable_tree_widget(self, obj, parent) -> QTreeWidgetItem: if not isinstance(obj, dict): @@ -657,6 +687,91 @@ def open_mat_file(self): if file_name: self.__load_mat_file(file_name) + def maintain_connection(self, root): + while self.realtimeConnectionEnabled: + if not self.signal_provider.establish_connection(): + self.realtimeConnectionEnabled = False + break + + # populate text logging tree + self.plottingLock.acquire() + if self.signal_provider.text_logging_data: + root = list(self.signal_provider.text_logging_data.keys())[0] + root_item = QTreeWidgetItem([root]) + root_item.setFlags(root_item.flags() & ~Qt.ItemIsSelectable) + items = self.__populate_text_logging_tree_widget( + self.signal_provider.text_logging_data[root], root_item + ) + self.ui.yarpTextLogTreeWidget.insertTopLevelItems(0, [items]) + if self.signal_provider.updateMetadata: + self.signal_provider.updateMetadata = False + root = list(self.signal_provider.data.keys())[0] + root_item = QTreeWidgetItem([root]) + root_item.setFlags(root_item.flags() & ~Qt.ItemIsSelectable) + items = self.__populate_variable_tree_widget( + self.signal_provider.data[root], root_item + ) + self.ui.variableTreeWidget.clear() + self.ui.variableTreeWidget.insertTopLevelItems(0, [items]) + + # spawn the console + self.pyconsole.push_local_ns("data", self.signal_provider.data) + + self.ui.timeSlider.setMaximum(self.signal_size) + + if len(self.plotData) > 0 and len(self.plotData) > self.ui.tabPlotWidget.currentIndex(): + self.plot_items[self.ui.tabPlotWidget.currentIndex()].canvas.update_plots( + self.plotData[self.ui.tabPlotWidget.currentIndex()]["paths"], + self.plotData[self.ui.tabPlotWidget.currentIndex()]["legends"], + self.realtimeConnectionEnabled) + self.plottingLock.release() + + time.sleep(self.animation_period + self.sleepPeriodBuffer) + self.meshcat_provider.updateMeshRealtime() + + def connect_realtime_logger(self): + self.realtimeConnectionEnabled = True + self.signal_provider.root_name = "robot_realtime" + print("Now connecting for real-time logging") + + # Do initial connection to populate the necessary data + if not self.signal_provider.establish_connection(): + print("Could not connect to YARP server, closing") + self.realtimeConnectionEnabled = False + return + self.meshcat_provider._realtimeMeshUpdate = True + # only display one root in the gui + root = list(self.signal_provider.data.keys())[0] + root_item = QTreeWidgetItem([root]) + root_item.setFlags(root_item.flags() & ~Qt.ItemIsSelectable) + items = self.__populate_variable_tree_widget( + self.signal_provider.data[root], root_item + ) + self.ui.variableTreeWidget.insertTopLevelItems(0, [items]) + + + # load the model + # self.signal_provider.joints_name = self.signal_provider.data["robot_realtime"]["description_list"]["elements_names"].tolist() + # self.signal_provider.robot_name = self.signal_provider.data["robot_realtime"]["yarp_robot_name"]["elements_names"][0] + if not self.meshcat_provider.load_model( + self.signal_provider.joints_name, self.signal_provider.robot_name + ): + # if not loaded we print an error but we continue + msg = "Unable to load the model: " + if self.meshcat_provider.custom_model_path: + msg = msg + self.meshcat_provider.custom_model_path + else: + msg = msg + self.signal_provider.robot_name + + self.logger.write_to_log(msg) + + # Disable these buttons for RT communication + self.ui.startButton.setEnabled(False) + self.ui.timeSlider.setEnabled(False) + self.networkThread = threading.Thread(target=self.maintain_connection, args=(root,)) + self.networkThread.start() + + def open_about(self): self.about.show() diff --git a/robot_log_visualizer/ui/plot_item.py b/robot_log_visualizer/ui/plot_item.py index b9ada3e..5bc7ca5 100644 --- a/robot_log_visualizer/ui/plot_item.py +++ b/robot_log_visualizer/ui/plot_item.py @@ -18,3 +18,9 @@ def __init__(self, signal_provider, period): parent=self, period=period, signal_provider=signal_provider ) self.ui.plotLayout.addWidget(self.canvas) + + def updatePlotItem(self, signal_provider, period): + self.canvas = MatplotlibViewerCanvas( + parent=self, period=period, signal_provider=signal_provider + ) + self.ui.plotLayout.addWidget(self.canvas)