RT Capabilities Part 1 - robot-log-visualizer

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,13 +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()
+            self.realtime_network_init = True
+            self.rt_metadata_dict = self.vector_collections_client.get_metadata().vectors
+            if not self.rt_metadata_dict:
+                return False
+
+            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)
+
+        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)
             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,35 @@ def on_pick(self, event):
                 blit=True,
             )
 
-    def update_plots(self, paths, legends):
+    def update_plots(self, paths, legends, realtime_plot):
+        if realtime_plot:
+            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 realtime_plot:
+                (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 +222,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 +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()

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

robot_log_visualizer/ui/autogenerated/visualizer.py

@@ -239,15 +239,25 @@ 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,7 +285,13 @@ def retranslateUi(self, MainWindow):
         self.actionQuit.setText(_translate("MainWindow", "&Quit"))
         self.actionQuit.setShortcut(_translate("MainWindow", "Ctrl+Q"))
         self.actionOpen.setText(_translate("MainWindow", "&Open"))
+        try:
+            import bipedal_locomotion_framework.bindings as blf
+            self.actionConnect.setText(_translate("MainWindow", "Realtime Connect"))
+        except ImportError:
+            self.actionConnect.setText(_translate("MainWindow", "Install BLF for RT Connect Functionality"))
         self.actionOpen.setShortcut(_translate("MainWindow", "Ctrl+O"))
         self.actionAbout.setText(_translate("MainWindow", "About"))
         self.actionSet_Robot_Model.setText(_translate("MainWindow", "Set Robot Model"))
+
 from PyQt5 import QtWebEngineWidgets