Skip to content

Commit

Permalink
šŸ› Fix the robot visualization when the dataset contains signals retriā€¦
Browse files Browse the repository at this point in the history 
ā€¦eved at different frequencies
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi committed Nov 15, 2023
1 parent e12cce5 commit 3c720d3
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 38 deletions.
16 changes: 16 additions & 0 deletions robot_log_visualizer/file_reader/signal_provider.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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()
Expand Down
10 changes: 3 additions & 7 deletions robot_log_visualizer/robot_visualizer/meshcat_provider.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
)

Expand Down
64 changes: 33 additions & 31 deletions robot_log_visualizer/ui/gui.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand All @@ -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
Expand All @@ -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())
Expand All @@ -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)
Expand All @@ -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())
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 3c720d3

Please sign in to comment.