🐛 Fix the robot visualization when the dataset contains signals retrieved at different frequencies (#72)

Author: GiulioRomualdi

robot_log_visualizer/file_reader/signal_provider.py

@@ -193,6 +193,9 @@ def index(self, index):
     def register_update_index(self, slot):
         self.update_index_signal.connect(slot)
 
+    def set_dataset_percentage(self, percentage):
+        self.update_index(int(percentage * len(self)))
+
     def update_index(self, index):
         locker = QMutexLocker(self.index_lock)
         self._index = max(min(index, len(self.timestamps) - 1), 0)
@@ -204,6 +207,19 @@ def current_time(self):
         value = self._current_time
         return value
 
+    def get_joints_position(self):
+        return self.data[self.root_name]["joints_state"]["positions"]["data"]
+
+    def get_joints_position_at_index(self, index):
+        joints_position_timestamps = self.data[self.root_name]["joints_state"][
+            "positions"
+        ]["timestamps"]
+        # given the index find the closest timestamp
+        closest_index = np.argmin(
+            np.abs(joints_position_timestamps - self.timestamps[index])
+        )
+        return self.get_joints_position()[closest_index, :]
+
     def run(self):
         while True:
             start = time.time()

robot_log_visualizer/robot_visualizer/meshcat_provider.py

@@ -149,16 +149,12 @@ def run(self):
 
             if self.state == PeriodicThreadState.running and self._is_model_loaded:
                 # These are the robot measured joint positions in radians
-                joints = self._signal_provider.data[self._signal_provider.root_name][
-                    "joints_state"
-                ]["positions"]["data"]
-
                 self.meshcat_visualizer.set_multibody_system_state(
                     base_position,
                     base_rotation,
-                    joint_value=joints[
-                        self._signal_provider.index, self.model_joints_index
-                    ],
+                    joint_value=self._signal_provider.get_joints_position_at_index(
+                        self._signal_provider.index
+                    )[self.model_joints_index],
                     model_name="robot",
                 )
 

robot_log_visualizer/ui/gui.py

@@ -246,7 +246,10 @@ def keyPressEvent(self, event):
             if event.key() == Qt.Key_B:
                 self.slider_pressed = True
                 new_index = int(self.ui.timeSlider.value()) - 1
-                self.signal_provider.update_index(new_index)
+                dataset_percentage = float(new_index) / float(
+                    self.ui.timeSlider.maximum()
+                )
+                self.signal_provider.set_dataset_percentage(dataset_percentage)
                 self.ui.timeLabel.setText(f"{self.signal_provider.current_time:.2f}")
                 self.text_logger.highlight_cell(
                     self.find_text_log_index(self.get_text_log_item_path())
@@ -255,11 +258,8 @@ def keyPressEvent(self, event):
                 # for every video item we set the instant
                 for video_item in self.video_items:
                     if video_item.media_loaded:
-                        video_percentage = float(new_index) / float(
-                            self.ui.timeSlider.maximum()
-                        )
                         video_item.media_player.setPosition(
-                            int(video_percentage * video_item.media_player.duration())
+                            int(dataset_percentage * video_item.media_player.duration())
                         )
 
                 # update the time slider
@@ -268,7 +268,10 @@ def keyPressEvent(self, event):
             elif event.key() == Qt.Key_F:
                 self.slider_pressed = True
                 new_index = int(self.ui.timeSlider.value()) + 1
-                self.signal_provider.update_index(new_index)
+                dataset_percentage = float(new_index) / float(
+                    self.ui.timeSlider.maximum()
+                )
+                self.signal_provider.set_dataset_percentage(dataset_percentage)
                 self.ui.timeLabel.setText(f"{self.signal_provider.current_time:.2f}")
                 self.text_logger.highlight_cell(
                     self.find_text_log_index(self.get_text_log_item_path())
@@ -277,11 +280,8 @@ def keyPressEvent(self, event):
                 # for every video item we set the instant
                 for video_item in self.video_items:
                     if video_item.media_loaded:
-                        video_percentage = float(new_index) / float(
-                            self.ui.timeSlider.maximum()
-                        )
                         video_item.media_player.setPosition(
-                            int(video_percentage * video_item.media_player.duration())
+                            int(dataset_percentage * video_item.media_player.duration())
                         )
 
                 self.ui.timeSlider.setValue(new_index)
@@ -303,31 +303,31 @@ def timeSlider_on_pressed(self):
 
     def timeSlider_on_sliderMoved(self):
         index = int(self.ui.timeSlider.value())
+        dataset_percentage = float(index) / float(self.ui.timeSlider.maximum())
 
         for video_item in self.video_items:
             if video_item.media_loaded:
-                video_percentage = float(index) / float(self.ui.timeSlider.maximum())
                 video_item.media_player.setPosition(
-                    int(video_percentage * video_item.media_player.duration())
+                    int(dataset_percentage * video_item.media_player.duration())
                 )
 
-        self.signal_provider.update_index(index)
+        self.signal_provider.set_dataset_percentage(dataset_percentage)
         self.ui.timeLabel.setText(f"{self.signal_provider.current_time:.2f}")
         self.text_logger.highlight_cell(
             self.find_text_log_index(self.get_text_log_item_path())
         )
 
     def timeSlider_on_release(self):
         index = int(self.ui.timeSlider.value())
+        dataset_percentage = float(index) / float(self.ui.timeSlider.maximum())
 
         for video_item in self.video_items:
             if video_item.media_loaded:
-                video_percentage = float(index) / float(self.ui.timeSlider.maximum())
                 video_item.media_player.setPosition(
-                    int(video_percentage * video_item.media_player.duration())
+                    int(dataset_percentage * video_item.media_player.duration())
                 )
 
-        self.signal_provider.update_index(index)
+        self.signal_provider.set_dataset_percentage(dataset_percentage)
         self.ui.timeLabel.setText(f"{self.signal_provider.current_time:.2f}")
         self.text_logger.highlight_cell(
             self.find_text_log_index(self.get_text_log_item_path())
@@ -475,22 +475,24 @@ def plotTabBar_currentChanged(self, index):
 
     @pyqtSlot()
     def update_index(self):
-        if not self.slider_pressed:
-            self.ui.timeSlider.setValue(self.signal_provider.index)
-            self.ui.timeLabel.setText(f"{self.signal_provider.current_time:.2f}")
-            self.text_logger.highlight_cell(
-                self.find_text_log_index(self.get_text_log_item_path())
-            )
+        if self.slider_pressed:
+            return
 
-            # TODO: this is a hack to update the video player and it should be done only for the activated videos
-            for video_item in self.video_items:
-                if video_item.media_loaded:
-                    video_percentage = float(self.ui.timeSlider.value()) / float(
-                        self.ui.timeSlider.maximum()
-                    )
-                    video_item.media_player.setPosition(
-                        int(video_percentage * video_item.media_player.duration())
-                    )
+        self.ui.timeSlider.setValue(self.signal_provider.index)
+        self.ui.timeLabel.setText(f"{self.signal_provider.current_time:.2f}")
+        self.text_logger.highlight_cell(
+            self.find_text_log_index(self.get_text_log_item_path())
+        )
+
+        # TODO: this is a hack to update the video player and it should be done only for the activated videos
+        for video_item in self.video_items:
+            if video_item.media_loaded:
+                video_percentage = float(self.ui.timeSlider.value()) / float(
+                    self.ui.timeSlider.maximum()
+                )
+                video_item.media_player.setPosition(
+                    int(video_percentage * video_item.media_player.duration())
+                )
 
     def closeEvent(self, event):
         # close the window