diff --git a/robot_log_visualizer/file_reader/signal_provider.py b/robot_log_visualizer/file_reader/signal_provider.py index 6d92ea2..1f46281 100644 --- a/robot_log_visualizer/file_reader/signal_provider.py +++ b/robot_log_visualizer/file_reader/signal_provider.py @@ -11,6 +11,10 @@ import idyntree.swig as idyn +# for real-time logging +import yarp + + class TextLoggingMsg: def __init__(self, level, text): self.level = level @@ -35,6 +39,13 @@ class SignalProvider(QThread): def __init__(self, period: float): QThread.__init__(self) + self.blfInstalled = True + try: + import bipedal_locomotion_framework.bindings as blf + self.blf = blf + except ImportError: + self.blfInstalled = False + # set device state self._state = PeriodicThreadState.pause self.state_lock = QMutex() @@ -67,7 +78,16 @@ def __init__(self, period: float): self._current_time = 0 + self.realtimeBufferReached = False + self.initMetadata = False + self.realtime_fixed_plot_window = 20 + + # for networking with the real-time logger + self.realtime_network_init = False + if self.blfInstalled: + self.vector_collections_client = blf.yarp_utilities.VectorsCollectionClient() self.trajectory_span = 200 + self.rt_metadata_dict = {} def __populate_text_logging_data(self, file_object): data = {} @@ -145,11 +165,103 @@ 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 __populate_realtime_logger_data(self, raw_data, keys, value, recent_timestamp): + if keys[0] not in raw_data: + raw_data[keys[0]] = {} + + if len(keys) == 1: + raw_data[keys[0]]["data"] = np.append(raw_data[keys[0]]["data"], value).reshape(-1, len(value)) + raw_data[keys[0]]["timestamps"] = np.append(raw_data[keys[0]]["timestamps"], recent_timestamp) + + temp_initial_time = raw_data[keys[0]]["timestamps"][0] + temp_end_time = raw_data[keys[0]]["timestamps"][-1] + while temp_end_time - temp_initial_time > self.realtime_fixed_plot_window: + raw_data[keys[0]]["data"] = np.delete(raw_data[keys[0]]["data"], 0, axis=0) + raw_data[keys[0]]["timestamps"] = np.delete(raw_data[keys[0]]["timestamps"], 0) + temp_initial_time = raw_data[keys[0]]["timestamps"][0] + temp_end_time = raw_data[keys[0]]["timestamps"][-1] + + else: + self.__populate_realtime_logger_data(raw_data[keys[0]], keys[1:], value, recent_timestamp) + + def __populate_realtime_logger_metadata(self, raw_data, keys, value): + if keys[0] == "timestamps": + return + if keys[0] not in raw_data: + raw_data[keys[0]] = {} + + if len(keys) == 1: + if len(value) == 0: + del raw_data[keys[0]] + return + if "elements_names" not in raw_data[keys[0]]: + raw_data[keys[0]]["elements_names"] = np.array([]) + raw_data[keys[0]]["data"] = np.array([]) + raw_data[keys[0]]["timestamps"] = np.array([]) + + raw_data[keys[0]]["elements_names"] = np.append(raw_data[keys[0]]["elements_names"], value) + else: + self.__populate_realtime_logger_metadata(raw_data[keys[0]], keys[1:], value) + + + def maintain_connection(self): + if not self.realtime_network_init: + yarp.Network.init() + + param_handler = self.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.vector_collections_client.initialize(param_handler) + + self.vector_collections_client.connect() + try: + self.rt_metadata_dict = self.vector_collections_client.get_metadata().vectors + except ValueError: + print("Error in retreiving the metadata from the logger") + print("Check if the logger is running and configured for realtime connection") + return False + + self.realtime_network_init = True + self.joints_name = self.rt_metadata_dict["robot_realtime::description_list"] + self.robot_name = self.rt_metadata_dict["robot_realtime::yarp_robot_name"][0] + for key_string, value in self.rt_metadata_dict.items(): + keys = key_string.split("::") + self.__populate_realtime_logger_metadata(self.data, keys, value) + del self.data["robot_realtime"]["description_list"] + del self.data["robot_realtime"]["yarp_robot_name"] + + vc_input = self.vector_collections_client.read_data(True).vectors + + if not vc_input: + return False + else: + # Update the timestamps + recent_timestamp = vc_input["robot_realtime::timestamps"][0] + self.timestamps = np.append(self.timestamps, recent_timestamp).reshape(-1) + del vc_input["robot_realtime::timestamps"] + + # Keep the data within the fixed time interval + while recent_timestamp - self.timestamps[0] > self.realtime_fixed_plot_window: + 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] + + # Store the new data that comes in + for key_string, value in vc_input.items(): + keys = key_string.split("::") + self.__populate_realtime_logger_data(self.data, keys, value, recent_timestamp) + + 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) diff --git a/robot_log_visualizer/plotter/matplotlib_viewer_canvas.py b/robot_log_visualizer/plotter/matplotlib_viewer_canvas.py index 84fd49f..5e4986f 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 @@ -184,30 +185,43 @@ def on_pick(self, event): blit=True, ) - def update_plots(self, paths, legends): + def update_plots(self, paths, legends, realtime_plot): + self.axes.cla() + colorIndex = 0 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"][:] + 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 + timestamps = data["timestamps"] - self.signal_provider.initial_time + if realtime_plot: (self.active_paths[path_string],) = self.axes.plot( timestamps, datapoints, label=legend_string, picker=True, - color=next(self.color_palette), + color=self.color_palette.get_color(colorIndex), ) + colorIndex = colorIndex + 1 + else: + (self.active_paths[path_string],) = self.axes.plot( + timestamps, + datapoints, + label=legend_string, + picker=True, + color=self.color_palette.get_color(colorIndex), + ) + colorIndex = colorIndex + 1 + paths_to_be_canceled = [] for active_path in self.active_paths.keys(): @@ -220,14 +234,20 @@ 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 realtime_plot: + #self.axes.autoscale() + self.axes.set_xlim(0, self.signal_provider.realtime_fixed_plot_window) + 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 + self.vertical_line_anim._stop() self.axes.legend() + self.axes.grid(True) if not self.frame_legend: self.frame_legend = self.axes.legend().get_frame() diff --git a/robot_log_visualizer/robot_visualizer/meshcat_provider.py b/robot_log_visualizer/robot_visualizer/meshcat_provider.py index 3288943..a723d17 100644 --- a/robot_log_visualizer/robot_visualizer/meshcat_provider.py +++ b/robot_log_visualizer/robot_visualizer/meshcat_provider.py @@ -224,3 +224,15 @@ def run(self): if self.state == PeriodicThreadState.closed: return + + # For the real-time logger + def update_mesh_realtime(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/about.py b/robot_log_visualizer/ui/autogenerated/about.py index fedcf29..61a927f 100644 --- a/robot_log_visualizer/ui/autogenerated/about.py +++ b/robot_log_visualizer/ui/autogenerated/about.py @@ -2,9 +2,10 @@ # Form implementation generated from reading ui file 'robot_log_visualizer/ui/misc/about.ui' # -# Created by: PyQt5 UI code generator 5.14.1 +# Created by: PyQt5 UI code generator 5.15.9 # -# WARNING! All changes made in this file will be lost! +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. from PyQt5 import QtCore, QtGui, QtWidgets diff --git a/robot_log_visualizer/ui/autogenerated/plot_tab.py b/robot_log_visualizer/ui/autogenerated/plot_tab.py index dc3fd67..1b1d496 100644 --- a/robot_log_visualizer/ui/autogenerated/plot_tab.py +++ b/robot_log_visualizer/ui/autogenerated/plot_tab.py @@ -2,9 +2,10 @@ # Form implementation generated from reading ui file 'robot_log_visualizer/ui/misc/plot_tab.ui' # -# Created by: PyQt5 UI code generator 5.14.1 +# Created by: PyQt5 UI code generator 5.15.9 # -# WARNING! All changes made in this file will be lost! +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. from PyQt5 import QtCore, QtGui, QtWidgets diff --git a/robot_log_visualizer/ui/autogenerated/set_robot_model.py b/robot_log_visualizer/ui/autogenerated/set_robot_model.py index f49c134..6100115 100644 --- a/robot_log_visualizer/ui/autogenerated/set_robot_model.py +++ b/robot_log_visualizer/ui/autogenerated/set_robot_model.py @@ -2,9 +2,10 @@ # Form implementation generated from reading ui file 'robot_log_visualizer/ui/misc/set_robot_model.ui' # -# Created by: PyQt5 UI code generator 5.14.1 +# Created by: PyQt5 UI code generator 5.15.9 # -# WARNING! All changes made in this file will be lost! +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. from PyQt5 import QtCore, QtGui, QtWidgets @@ -61,8 +62,8 @@ def setupUi(self, setRobotModelDialog): self.gridLayout.addItem(spacerItem, 2, 0, 1, 1) self.retranslateUi(setRobotModelDialog) - self.buttonBox.accepted.connect(setRobotModelDialog.accept) - self.buttonBox.rejected.connect(setRobotModelDialog.reject) + self.buttonBox.accepted.connect(setRobotModelDialog.accept) # type: ignore + self.buttonBox.rejected.connect(setRobotModelDialog.reject) # type: ignore QtCore.QMetaObject.connectSlotsByName(setRobotModelDialog) def retranslateUi(self, setRobotModelDialog): diff --git a/robot_log_visualizer/ui/autogenerated/video_tab.py b/robot_log_visualizer/ui/autogenerated/video_tab.py index 0a0c3c0..f627445 100644 --- a/robot_log_visualizer/ui/autogenerated/video_tab.py +++ b/robot_log_visualizer/ui/autogenerated/video_tab.py @@ -2,9 +2,10 @@ # Form implementation generated from reading ui file 'robot_log_visualizer/ui/misc/video_tab.ui' # -# Created by: PyQt5 UI code generator 5.14.1 +# Created by: PyQt5 UI code generator 5.15.9 # -# WARNING! All changes made in this file will be lost! +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. from PyQt5 import QtCore, QtGui, QtWidgets diff --git a/robot_log_visualizer/ui/autogenerated/visualizer.py b/robot_log_visualizer/ui/autogenerated/visualizer.py index 7e3c9be..373bfaa 100644 --- a/robot_log_visualizer/ui/autogenerated/visualizer.py +++ b/robot_log_visualizer/ui/autogenerated/visualizer.py @@ -2,9 +2,10 @@ # Form implementation generated from reading ui file 'robot_log_visualizer/ui/misc/visualizer.ui' # -# Created by: PyQt5 UI code generator 5.14.1 +# Created by: PyQt5 UI code generator 5.15.9 # -# WARNING! All changes made in this file will be lost! +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. from PyQt5 import QtCore, QtGui, QtWidgets @@ -247,7 +248,10 @@ def setupUi(self, MainWindow): self.actionAbout.setObjectName("actionAbout") self.actionSet_Robot_Model = QtWidgets.QAction(MainWindow) self.actionSet_Robot_Model.setObjectName("actionSet_Robot_Model") + self.actionRealtime_Connect = QtWidgets.QAction(MainWindow) + self.actionRealtime_Connect.setObjectName("actionRealtime_Connect") self.menuFile.addAction(self.actionOpen) + self.menuFile.addAction(self.actionRealtime_Connect) self.menuFile.addSeparator() self.menuFile.addAction(self.actionQuit) self.menuHelp.addAction(self.actionAbout) @@ -278,4 +282,6 @@ def retranslateUi(self, MainWindow): self.actionOpen.setShortcut(_translate("MainWindow", "Ctrl+O")) self.actionAbout.setText(_translate("MainWindow", "About")) self.actionSet_Robot_Model.setText(_translate("MainWindow", "Set Robot Model")) + self.actionRealtime_Connect.setText(_translate("MainWindow", "Realtime Connect")) + self.actionRealtime_Connect.setShortcut(_translate("MainWindow", "Ctrl+R")) from PyQt5 import QtWebEngineWidgets diff --git a/robot_log_visualizer/ui/gui.py b/robot_log_visualizer/ui/gui.py index fdc77a7..51ba323 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.realtime_connection_enabled = False + self.timeoutAttempts = 20 + self.sleepPeriodBuffer = 0.02 + self.animation_period = animation_period # set up the user interface @@ -185,6 +198,11 @@ 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) + + if self.signal_provider.blfInstalled: + self.ui.actionRealtime_Connect.triggered.connect(self.connect_realtime_logger) + else: + self.ui.actionRealtime_Connect.setText("Install BLF for RT Connect Functionality") self.ui.actionAbout.triggered.connect(self.open_about) self.ui.actionSet_Robot_Model.triggered.connect(self.open_set_robot_model) @@ -239,6 +257,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 +321,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 +372,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 +385,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 +436,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.realtime_connection_enabled ) + self.plottingLock.release() def find_text_log_index(self, path): current_time = self.signal_provider.current_time @@ -535,6 +565,9 @@ def closeEvent(self, event): self.signal_provider.wait() event.accept() + if self.realtime_connection_enabled: + self.realtime_connection_enabled = False + self.network_thread.join() def __populate_variable_tree_widget(self, obj, parent) -> QTreeWidgetItem: if not isinstance(obj, dict): @@ -664,6 +697,80 @@ def open_mat_file(self): if file_name: self.__load_mat_file(file_name) + def establish_connection(self, root): + while self.realtime_connection_enabled: + if not self.signal_provider.maintain_connection(): + self.realtime_connection_enabled = 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]) + # 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.realtime_connection_enabled) + self.plottingLock.release() + + time.sleep(self.animation_period + self.sleepPeriodBuffer) + self.meshcat_provider.update_mesh_realtime() + + def connect_realtime_logger(self): + self.realtime_connection_enabled = True + self.signal_provider.root_name = "robot_realtime" + + # Do initial connection to populate the necessary data + if not self.signal_provider.maintain_connection(): + self.logger.write_to_log("Could not connect to the real-time logger.") + self.realtime_connection_enabled = False + self.signal_provider.root_name = "robot_logger_device" + 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.network_thread = threading.Thread(target=self.establish_connection, args=(root,)) + self.network_thread.start() + + def open_about(self): self.about.show() diff --git a/robot_log_visualizer/ui/misc/visualizer.ui b/robot_log_visualizer/ui/misc/visualizer.ui index c3dff71..9696a61 100644 --- a/robot_log_visualizer/ui/misc/visualizer.ui +++ b/robot_log_visualizer/ui/misc/visualizer.ui @@ -503,6 +503,7 @@ &File + @@ -556,12 +557,20 @@ Set Robot Model + + + Realtime Connect + + + Ctrl+R + + QWebEngineView QWidget -
PyQt5.QtWebEngineWidgets
+
QtWebEngineWidgets/QWebEngineView
1