RT Capabilities Part 1 - robot-log-visualizer

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,15 @@ 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 = {}
 
     def __populate_text_logging_data(self, file_object):
         data = {}
@@ -121,8 +136,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 +162,105 @@ 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 maintain_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().vectors
+            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"]
+
+        vc_input = self.vectorCollectionsClient.read_data(True)
+
+        if not vc_input:
+            print("Failed to read realtime YARP port, closing")
+            return False
+        else:
+            # Update the timestamps
+            recentTimestamp = vc_input["robot_realtime::timestamps"][0]
+            self.timestamps = np.append(self.timestamps, recentTimestamp).reshape(-1)
+            del vc_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]
+
+            # Store the new data that comes in
+            for keyString, value in vc_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
@@ -184,23 +185,34 @@ def on_pick(self, event):
                 blit=True,
             )
 
-    def update_plots(self, paths, legends):
+    def update_plots(self, paths, legends, realtimePlot):
+        self.axes.cla()
+        realtimeColorIndex = 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 realtimePlot:
+                (self.active_paths[path_string],) = self.axes.plot(
+                    timestamps,
+                    datapoints,
+                    label=legend_string,
+                    picker=True,
+                    color=self.color_palette.get_color(realtimeColorIndex),
+                )
+                realtimeColorIndex = realtimeColorIndex + 1
+            else:
                 (self.active_paths[path_string],) = self.axes.plot(
                     timestamps,
                     datapoints,
@@ -209,6 +221,7 @@ def update_plots(self, paths, legends):
                     color=next(self.color_palette),
                 )
 
+
         paths_to_be_canceled = []
         for active_path in self.active_paths.keys():
             path = active_path.split("/")
@@ -220,14 +233,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 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
+
         self.vertical_line_anim._stop()
         self.axes.legend()
+        self.axes.grid()
 
         if not self.frame_legend:
             self.frame_legend = self.axes.legend().get_frame()

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