Add real-time capability to visualizer (part 1) #100

Co-authored-by: Giulio Romualdi <giulio.romualdi@gmail.com>
Co-authored-by: Nick <nicholas.tremaroli@iit.it>

Author: 3 people

robot_log_visualizer/__main__.py

@@ -5,14 +5,11 @@
 # Released under the terms of the BSD 3-Clause License
 
 import sys
-import os
 
 # GUI
 from robot_log_visualizer.ui.gui import RobotViewerMainWindow
 from PyQt5.QtWidgets import QApplication
 
-from robot_log_visualizer.file_reader.signal_provider import SignalProvider
-
 # Meshcat
 from robot_log_visualizer.robot_visualizer.meshcat_provider import MeshcatProvider
 
@@ -24,29 +21,21 @@ def main():
         "plot_animation": 0.03,
     }
 
-    # instantiate device_manager
-    signal_provider = SignalProvider(period=thread_periods["signal_provider"])
-
-    meshcat_provider = MeshcatProvider(
-        period=thread_periods["meshcat_provider"], signal_provider=signal_provider
-    )
+    meshcat_provider = MeshcatProvider(period=thread_periods["meshcat_provider"])
 
     # instantiate a QApplication
     app = QApplication(sys.argv)
 
     # instantiate the main window
     gui = RobotViewerMainWindow(
-        signal_provider=signal_provider,
+        signal_provider_period=thread_periods["signal_provider"],
         meshcat_provider=meshcat_provider,
         animation_period=thread_periods["plot_animation"],
     )
 
     # show the main window
     gui.show()
 
-    signal_provider.start()
-    meshcat_provider.start()
-
     return app.exec_()
 
 

robot_log_visualizer/plotter/pyqtgraph_viewer_canvas.py

@@ -4,13 +4,14 @@
 
 from __future__ import annotations
 
-from typing import Dict, Tuple, Sequence, Iterable
+from typing import Dict, Iterable, Sequence, Tuple
 
-from PyQt5 import QtCore, QtWidgets  # type: ignore
-import pyqtgraph as pg  # type: ignore
 import numpy as np
+import pyqtgraph as pg  # type: ignore
+from PyQt5 import QtCore, QtWidgets  # type: ignore
 
 from robot_log_visualizer.plotter.color_palette import ColorPalette
+from robot_log_visualizer.signal_provider.signal_provider import ProviderType
 
 # ------------------------------------------------------------------------
 # Type aliases
@@ -31,7 +32,6 @@ class PyQtGraphViewerCanvas(QtWidgets.QWidget):
     def __init__(
         self,
         parent: QtWidgets.QWidget | None,
-        signal_provider,
         period: float,
         *,
         click_radius: float | None = None,
@@ -41,16 +41,14 @@ def __init__(
 
         Args:
             parent: Parent widget or *None*.
-            signal_provider: Object exposing ``data``, ``initial_time``,
-                ``end_time`` and a **dynamic** ``current_time`` attribute.
             period: Update period for the vertical line (seconds).
             click_radius: Override :pyattr:`DEFAULT_RADIUS`.
             marker_size: Override :pyattr:`DEFAULT_MARKER_SIZE`.
         """
         super().__init__(parent)
 
         # injected dependencies
-        self._signal_provider = signal_provider
+        self._signal_provider = None
         self._period_ms: int = int(period * 1000)
         self._click_radius: float = click_radius or self.DEFAULT_RADIUS
         self._marker_size: int = marker_size or self.DEFAULT_MARKER_SIZE
@@ -66,21 +64,58 @@ def __init__(
         self._connect_signals()
 
     # -------------------------------------------------------------#
-    # Public API (called from the outside)                                  #
+    # Public API (called from the outside)                         #
     # -------------------------------------------------------------#
+
+    def set_signal_provider(self, signal_provider) -> None:
+        """Set the signal provider to fetch data from.
+
+        Args:
+            signal_provider: An instance of `SignalProvider`.
+        """
+
+        if signal_provider is None:
+            return
+
+        self._signal_provider = signal_provider
+
+        # Connect to real-time updates for real-time provider
+        if self._signal_provider.provider_type == ProviderType.REALTIME:
+            self._signal_provider.update_index_signal.connect(
+                self._update_realtime_curves
+            )
+
     def update_plots(self, paths: Sequence[Path], legends: Sequence[Legend]) -> None:
         """Synchronise plots with the *paths* list.
 
         New items are added, disappeared items removed. Existing ones are
         left untouched to avoid flicker.
         """
+        if self._signal_provider is None:
+            return
+
+        # For real-time provider, update the set of selected signals to buffer
+        if self._signal_provider.provider_type == ProviderType.REALTIME:
+            selected_keys = ["::".join(path) for path in paths]
+            self._signal_provider.add_signals_to_buffer(selected_keys)
+
         self._add_missing_curves(paths, legends)
         self._remove_obsolete_curves(paths)
-        # Always show the full time span
-        self._plot.setXRange(
-            0.0,
-            self._signal_provider.end_time - self._signal_provider.initial_time,
-        )
+
+        # Set the X axis range based on the provider type
+        if self._signal_provider.provider_type == ProviderType.REALTIME:
+            # For real-time data, show a fixed window with 0 set at the right edge for the latest data
+            self._plot.setXRange(-self._signal_provider.realtime_fixed_plot_window, 0.0)
+            # Disable mouse panning on x axis
+            self._plot.plotItem.vb.setMouseEnabled(x=False, y=True)
+            # For real-time data enable autoscaling of Y axis
+            self._plot.plotItem.vb.enableAutoRange(axis=pg.ViewBox.YAxis, enable=True)
+        else:
+            # Default behavior
+            self._plot.setXRange(
+                0.0,
+                self._signal_provider.end_time - self._signal_provider.initial_time,
+            )
 
     # The following trio is wired to whoever controls the replay/stream
     def pause_animation(self) -> None:  # noqa: D401
@@ -173,8 +208,32 @@ def _remove_obsolete_curves(self, paths: Sequence[Path]) -> None:
 
     def _update_vline(self) -> None:
         """Move the vertical line to ``current_time``."""
+        if self._signal_provider is None:
+            return
+
         self._vline.setValue(self._signal_provider.current_time)
 
+    def _update_realtime_curves(self):
+        """Update all curves with the latest data from the signal provider."""
+
+        if self._signal_provider is None:
+            return
+        for key, curve in self._curves.items():
+            # Drill down to the data array using the path
+            path = key.split("/")
+            data = self._signal_provider.data
+            for subkey in path[:-1]:
+                data = data[subkey]
+            try:
+                y = data["data"][:, int(path[-1])]
+            except (IndexError, ValueError):
+                y = data["data"][:]
+
+            # Set the 0 of the x axis to the latest timestamp
+            latest_time = data["timestamps"][-1] if len(data["timestamps"]) > 0 else 0
+            x = data["timestamps"] - latest_time
+            curve.setData(x, y)
+
     def _on_mouse_click(self, event) -> None:  # noqa: N802
         """Handle a left‑click: select or unselect the nearest data point."""
         if event.button() != QtCore.Qt.MouseButton.LeftButton:

robot_log_visualizer/robot_visualizer/meshcat_provider.py

@@ -2,23 +2,21 @@
 # This software may be modified and distributed under the terms of the
 # Released under the terms of the BSD 3-Clause License
 
-from PyQt5.QtCore import QThread, QMutex, QMutexLocker
-
 import os
 import re
-from pathlib import Path
-
-import numpy as np
 import time
+from pathlib import Path
 
 import idyntree.swig as idyn
+import numpy as np
 from idyntree.visualize import MeshcatVisualizer
+from PyQt5.QtCore import QMutex, QMutexLocker, QThread
 
 from robot_log_visualizer.utils.utils import PeriodicThreadState
 
 
 class MeshcatProvider(QThread):
-    def __init__(self, signal_provider, period):
+    def __init__(self, period):
         QThread.__init__(self)
 
         self._state = PeriodicThreadState.pause
@@ -29,12 +27,17 @@ def __init__(self, signal_provider, period):
         self.meshcat_visualizer_mutex = QMutex()
 
         self._is_model_loaded = False
-        self._signal_provider = signal_provider
+        self._signal_provider = None
 
         self.model_path = ""
         self.custom_package_dir = ""
         self.base_frame = ""
-        self.env_list = ["GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH", "AMENT_PREFIX_PATH"]
+        self.env_list = [
+            "GAZEBO_MODEL_PATH",
+            "GZ_SIM_RESOURCE_PATH",
+            "ROS_PACKAGE_PATH",
+            "AMENT_PREFIX_PATH",
+        ]
         self._registered_3d_points = set()
         self._registered_3d_trajectories = dict()
         self._register_3d_arrow = set()
@@ -87,6 +90,9 @@ def unregister_3d_arrow(self, arrow_path):
         self._register_3d_arrow.remove(arrow_path)
         self._meshcat_visualizer.delete(shape_name=arrow_path)
 
+    def set_signal_provider(self, signal_provider):
+        self._signal_provider = signal_provider
+
     def load_model(self, considered_joints, model_name, base_frame=None):
         def get_model_path_from_envs(env_list):
             return [
@@ -148,7 +154,7 @@ def find_model_joints(model_name, considered_joints):
                     model_filenames = [
                         folder_model_path / Path(f)
                         for f in os.listdir(folder_model_path.absolute())
-                        if re.search("[a-zA-Z0-9_]*\.urdf", f)
+                        if re.search(r"[a-zA-Z0-9_]*\.urdf", f)
                     ]
 
                     if model_filenames:
@@ -158,6 +164,9 @@ def find_model_joints(model_name, considered_joints):
 
             # If the model is not found we exit
             if not model_found_in_env_folders:
+                print(
+                    f"MeshcatProvider: Unable to find the model {model_name} in the environment folders."
+                )
                 return False
 
             self.model_joints_index = find_model_joints(
@@ -234,6 +243,9 @@ def run(self):
 
             index = self._signal_provider.index
             if self.state == PeriodicThreadState.running and self._is_model_loaded:
+                if len(self._signal_provider) == 0:
+                    time.sleep(self._period)
+                    continue
                 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