Added Vector Collection Client to Read Real-Time Information

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)
 

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())

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",
+        )

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"))