diff --git a/robot_log_visualizer/file_reader/signal_provider.py b/robot_log_visualizer/file_reader/signal_provider.py index 18de7c4..d7ba996 100644 --- a/robot_log_visualizer/file_reader/signal_provider.py +++ b/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() diff --git a/robot_log_visualizer/robot_visualizer/meshcat_provider.py b/robot_log_visualizer/robot_visualizer/meshcat_provider.py index 3072b26..2a58dcf 100644 --- a/robot_log_visualizer/robot_visualizer/meshcat_provider.py +++ b/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", ) diff --git a/robot_log_visualizer/ui/gui.py b/robot_log_visualizer/ui/gui.py index 8bb0f16..a581b4f 100644 --- a/robot_log_visualizer/ui/gui.py +++ b/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,15 +303,15 @@ 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()) @@ -319,15 +319,15 @@ def timeSlider_on_sliderMoved(self): 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