From 5ad8e4e1584a0fcfce4b8f78d080f3a3db93a6b2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Sep 2024 17:20:47 +0200 Subject: [PATCH 1/4] Add the possibility to draw 3d arrows --- .../file_reader/signal_provider.py | 30 ++++++++++++++++++ .../robot_visualizer/meshcat_provider.py | 27 ++++++++++++++++ robot_log_visualizer/ui/gui.py | 31 +++++++++++++++++-- 3 files changed, 86 insertions(+), 2 deletions(-) diff --git a/robot_log_visualizer/file_reader/signal_provider.py b/robot_log_visualizer/file_reader/signal_provider.py index 6d92ea2..a5f30dd 100644 --- a/robot_log_visualizer/file_reader/signal_provider.py +++ b/robot_log_visualizer/file_reader/signal_provider.py @@ -51,6 +51,10 @@ def __init__(self, period: float): self._3d_trajectories_path = {} self._3d_trajectories_path_lock = QMutex() + self._3d_arrows = {} + self._3d_arrows_path_lock = QMutex() + self.max_arrow = 0 + self.period = period self.data = {} @@ -319,6 +323,18 @@ def get_3d_point_at_index(self, index): return points + def get_3d_arrow_at_index(self, index): + arrows = {} + + self._3d_arrows_path_lock.lock() + + for key, value in self._3d_arrows.items(): + arrows[key] = self.get_item_from_path_at_index(value, index) + + self._3d_arrows_path_lock.unlock() + + return arrows + def get_3d_trajectory_at_index(self, index): trajectories = {} @@ -354,6 +370,20 @@ def unregister_3d_point(self, key): del self._3d_points_path[key] self._3d_points_path_lock.unlock() + def register_3d_arrow(self, key, arrow_path): + self._3d_arrows_path_lock.lock() + self._3d_arrows[key] = arrow_path + for _, value in self._3d_arrows.items(): + data, _ = self.get_item_from_path(arrow_path) + arrow = data[:, 3:] + self.max_arrow = max(np.max(np.linalg.norm(arrow, axis=1)), self.max_arrow) + self._3d_arrows_path_lock.unlock() + + def unregister_3d_arrow(self, key): + self._3d_arrows_path_lock.lock() + del self._3d_arrows[key] + self._3d_arrows_path_lock.unlock() + def register_3d_trajectory(self, key, trajectory_path): self._3d_trajectories_path_lock.lock() self._3d_trajectories_path[key] = trajectory_path diff --git a/robot_log_visualizer/robot_visualizer/meshcat_provider.py b/robot_log_visualizer/robot_visualizer/meshcat_provider.py index 3288943..e19e174 100644 --- a/robot_log_visualizer/robot_visualizer/meshcat_provider.py +++ b/robot_log_visualizer/robot_visualizer/meshcat_provider.py @@ -36,6 +36,7 @@ def __init__(self, signal_provider, period): self.env_list = ["GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH", "AMENT_PREFIX_PATH"] self._registered_3d_points = set() self._registered_3d_trajectories = dict() + self._register_3d_arrow = set() @property def state(self): @@ -56,6 +57,14 @@ def register_3d_point(self, point_path, color): radius=radius, color=color, shape_name=point_path ) + def register_3d_arrow(self, arrow_path, color): + radius = 0.02 + locker = QMutexLocker(self.meshcat_visualizer_mutex) + self._register_3d_arrow.add(arrow_path) + self._meshcat_visualizer.load_arrow( + radius=radius, color=color, shape_name=arrow_path + ) + def register_3d_trajectory(self, trajectory_path, color): locker = QMutexLocker(self.meshcat_visualizer_mutex) self._registered_3d_trajectories[trajectory_path] = (False, color) @@ -70,6 +79,11 @@ def unregister_3d_trajectory(self, trajectory_path): self._registered_3d_trajectories.pop(trajectory_path, None) self._meshcat_visualizer.delete(shape_name=trajectory_path) + def unregister_3d_arrow(self, arrow_path): + locker = QMutexLocker(self.meshcat_visualizer_mutex) + self._register_3d_arrow.remove(arrow_path) + self._meshcat_visualizer.delete(shape_name=arrow_path) + def load_model(self, considered_joints, model_name): def get_model_path_from_envs(env_list): return [ @@ -216,6 +230,19 @@ def run(self): color=self._registered_3d_trajectories[trajectory_path][1], ) + for ( + arrow_path, + arrow, + ) in self._signal_provider.get_3d_arrow_at_index(index).items(): + if arrow_path not in self._register_3d_arrow: + continue + + self._meshcat_visualizer.set_arrow_transform( + origin=arrow[0:3], + vector=arrow[3:6] / self._signal_provider.max_arrow, + shape_name=arrow_path, + ) + self.meshcat_visualizer_mutex.unlock() sleep_time = self._period - (time.time() - start) diff --git a/robot_log_visualizer/ui/gui.py b/robot_log_visualizer/ui/gui.py index fdc77a7..c99092a 100644 --- a/robot_log_visualizer/ui/gui.py +++ b/robot_log_visualizer/ui/gui.py @@ -166,6 +166,7 @@ def __init__(self, signal_provider, meshcat_provider, animation_period): self.video_items = [] self.visualized_3d_points = set() self.visualized_3d_trajectories = set() + self.visualized_3d_arrows = set() self.visualized_3d_points_colors_palette = ColorPalette() self.toolButton_on_click() @@ -731,8 +732,10 @@ def variableTreeWidget_on_right_click(self, item_position): add_3d_point_str = "Show as a 3D point" add_3d_trajectory_str = "Show as a 3D trajectory" + add_3d_arrow_str = "Show as a 3D arrow" remove_3d_point_str = "Remove the 3D point" remove_3d_trajectory_str = "Remove the 3D trajectory" + remove_3d_arrow_str = "Remove the 3D arrow" use_as_base_position_str = "Use as base position" use_as_base_orientation_str = "Use as base orientation" dont_use_as_base_position_str = "Don't use as base position" @@ -784,6 +787,12 @@ def variableTreeWidget_on_right_click(self, item_position): else: menu.addAction(use_as_base_orientation_str + " (xyzw Quaternion)") + if item_size == 6: + if item_key in self.visualized_3d_arrows: + menu.addAction(remove_3d_arrow_str) + else: + menu.addAction(add_3d_arrow_str) + # show the menu action = menu.exec_(self.ui.variableTreeWidget.mapToGlobal(item_position)) if action is None: @@ -791,7 +800,11 @@ def variableTreeWidget_on_right_click(self, item_position): item_path = self.get_item_path(item) - if action.text() == add_3d_point_str or action.text() == add_3d_trajectory_str: + if ( + action.text() == add_3d_point_str + or action.text() == add_3d_trajectory_str + or action.text() == add_3d_arrow_str + ): color = next(self.visualized_3d_points_colors_palette) item.setForeground(0, QtGui.QBrush(QtGui.QColor(color.as_hex()))) @@ -802,12 +815,20 @@ def variableTreeWidget_on_right_click(self, item_position): ) self.signal_provider.register_3d_point(item_key, item_path) self.visualized_3d_points.add(item_key) - else: + elif action.text() == add_3d_trajectory_str: self.meshcat_provider.register_3d_trajectory( item_key, list(color.as_normalized_rgb()) ) self.signal_provider.register_3d_trajectory(item_key, item_path) self.visualized_3d_trajectories.add(item_key) + elif action.text() == add_3d_arrow_str: + self.meshcat_provider.register_3d_arrow( + item_key, list(color.as_normalized_rgb()) + ) + self.signal_provider.register_3d_arrow(item_key, item_path) + self.visualized_3d_arrows.add(item_key) + else: + raise ValueError("Unknown action") if action.text() == remove_3d_point_str: self.meshcat_provider.unregister_3d_point(item_key) @@ -821,6 +842,12 @@ def variableTreeWidget_on_right_click(self, item_position): self.visualized_3d_trajectories.remove(item_key) item.setForeground(0, QtGui.QBrush(QtGui.QColor(0, 0, 0))) + if action.text() == remove_3d_arrow_str: + self.meshcat_provider.unregister_3d_arrow(item_key) + self.signal_provider.unregister_3d_arrow(item_key) + self.visualized_3d_arrows.remove(item_key) + item.setForeground(0, QtGui.QBrush(QtGui.QColor(0, 0, 0))) + if ( use_as_base_orientation_str in action.text() or action.text() == use_as_base_position_str From fea73f4f6f3f8b970d230a3f3a65adcd5df552d8 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Sep 2024 17:29:41 +0200 Subject: [PATCH 2/4] Remove depracated GitHub actions --- .github/workflows/ci.yml | 2 +- .github/workflows/pypi-ci.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b5385e4..581bec0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -42,7 +42,7 @@ jobs: bash ./generate-ui.sh - name: Archive artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: autogenerated_files path: robot_log_visualizer/ui/autogenerated diff --git a/.github/workflows/pypi-ci.yml b/.github/workflows/pypi-ci.yml index 24470b4..23f4819 100644 --- a/.github/workflows/pypi-ci.yml +++ b/.github/workflows/pypi-ci.yml @@ -40,7 +40,7 @@ jobs: run: python -m build --sdist --wheel --outdir dist/ . - name: Archive artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: dist path: dist From 65a66653660c936b14e451c05607e8c8ed4b6abf Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 30 May 2025 12:34:32 +0200 Subject: [PATCH 3/4] =?UTF-8?q?=E2=9C=A8=20Enhance=20SignalProvider=20and?= =?UTF-8?q?=20GUI=20for=20custom=20arrow=20scaling=20functionality?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../file_reader/signal_provider.py | 26 +- robot_log_visualizer/ui/gui.py | 78 ++++- .../ui/misc/set_robot_model.ui | 275 +++++++++++------- robot_log_visualizer/ui/misc/visualizer.ui | 6 +- 4 files changed, 279 insertions(+), 106 deletions(-) diff --git a/robot_log_visualizer/file_reader/signal_provider.py b/robot_log_visualizer/file_reader/signal_provider.py index 82c2def..3b5d55b 100644 --- a/robot_log_visualizer/file_reader/signal_provider.py +++ b/robot_log_visualizer/file_reader/signal_provider.py @@ -53,7 +53,11 @@ def __init__(self, period: float): self._3d_arrows = {} self._3d_arrows_path_lock = QMutex() - self.max_arrow = 0 + + self._max_arrow = 0 + self._custom_max_arrow = 0 + self._is_custom_max_arrow_used = False + self._max_arrow_mutex = QMutex() self.period = period @@ -213,6 +217,14 @@ def robot_state_path(self): locker = QMutexLocker(self.robot_state_path_lock) value = self._robot_state_path return value + + @property + def max_arrow(self): + locker = QMutexLocker(self._max_arrow_mutex) + if self._is_custom_max_arrow_used: + return self._custom_max_arrow + else: + return self._max_arrow @robot_state_path.setter def robot_state_path(self, robot_state_path): @@ -308,6 +320,14 @@ def get_robot_state_at_index(self, index): return robot_state + def set_custom_max_arrow(self, use_custom_max_arrow: bool, max_arrow: float): + _ = QMutexLocker(self._max_arrow_mutex) + self._is_custom_max_arrow_used = use_custom_max_arrow + if use_custom_max_arrow: + self._custom_max_arrow = max_arrow + else: + self._custom_max_arrow = 0 + def get_3d_point_at_index(self, index): points = {} @@ -378,7 +398,9 @@ def register_3d_arrow(self, key, arrow_path): for _, value in self._3d_arrows.items(): data, _ = self.get_item_from_path(arrow_path) arrow = data[:, 3:] - self.max_arrow = max(np.max(np.linalg.norm(arrow, axis=1)), self.max_arrow) + self._max_arrow_mutex.lock() + self._max_arrow = max(np.max(np.linalg.norm(arrow, axis=1)), self._max_arrow) + self._max_arrow_mutex.unlock() self._3d_arrows_path_lock.unlock() def unregister_3d_arrow(self, key): diff --git a/robot_log_visualizer/ui/gui.py b/robot_log_visualizer/ui/gui.py index 2ef0236..efb3db5 100644 --- a/robot_log_visualizer/ui/gui.py +++ b/robot_log_visualizer/ui/gui.py @@ -71,6 +71,16 @@ def __init__(self, meshcat_provider, parent=None, dataset_loaded=False): self.ui.robotModelToolButton.clicked.connect(self.open_urdf_file) self.ui.packageDirToolButton.clicked.connect(self.open_package_directory) + # Force the arrowScaling_lineEdit to be a positive float + self.ui.arrowScaling_lineEdit.setValidator(QtGui.QDoubleValidator(0, 100, 2)) + + # connect the arrowScaling_checkBox to the handle_arrow_scaling method + self.ui.arrowScaling_checkBox.toggled.connect(self.handle_arrow_scaling) + + self.clicked_button = None + self.std_button = None + self.ui.buttonBox.clicked.connect(self.buttonBox_on_click) + if dataset_loaded: frames = meshcat_provider.robot_frames() self.ui.frameNameComboBox.addItems(frames) @@ -93,7 +103,33 @@ def get_urdf_path(self): def get_package_directory(self): return self.ui.packageDirLineEdit.text() + + def buttonBox_on_click(self, button): + self.clicked_button = button + + self.std_button = self.ui.buttonBox.standardButton(button) + def get_clicked_button_role(self): + if self.clicked_button is not None: + return self.ui.buttonBox.buttonRole(self.clicked_button) + return None + + def get_clicked_button_text(self): + if self.clicked_button is not None: + return self.clicked_button.text() + return None + + def get_clicked_standard_button(self): + return self.std_button + + def handle_arrow_scaling(self): + # if arrowScaling_checkBox is checked the lineEdit must be disabled else it must be enabled + if self.ui.arrowScaling_checkBox.isChecked(): + self.ui.arrowScaling_lineEdit.setText("") + self.ui.arrowScaling_lineEdit.setEnabled(False) + else: + self.ui.arrowScaling_lineEdit.setText("") + self.ui.arrowScaling_lineEdit.setEnabled(True) class About(QtWidgets.QMainWindow): def __init__(self): @@ -682,9 +718,45 @@ def open_set_robot_model(self): ) outcome = dlg.exec() if outcome == QDialog.Accepted: - if not self.dataset_loaded: - self.meshcat_provider.model_path = dlg.get_urdf_path() - self.meshcat_provider.custom_package_dir = dlg.get_package_directory() + + # check which button was clicked + button_role = dlg.get_clicked_button_role() + button_text = dlg.get_clicked_button_text() + std_button = dlg.get_clicked_standard_button() + + if std_button == QtWidgets.QDialogButtonBox.SaveAll: + if not self.dataset_loaded: + self.meshcat_provider.model_path = dlg.get_urdf_path() + self.meshcat_provider.custom_package_dir = dlg.get_package_directory() + + + arrow_scaling_value = dlg.ui.arrowScaling_lineEdit.text() + if not arrow_scaling_value: + arrow_scaling_value = "1.0" + else: + arrow_scaling_value = float(arrow_scaling_value) + self.signal_provider.set_custom_max_arrow( + not dlg.ui.arrowScaling_checkBox.isChecked(), + arrow_scaling_value + ) + if std_button == QtWidgets.QDialogButtonBox.Save: + # we need to check which tab is selected in the dlg + if dlg.ui.tabWidget.currentIndex() == 0: + if not self.dataset_loaded: + self.meshcat_provider.model_path = dlg.get_urdf_path() + self.meshcat_provider.custom_package_dir = dlg.get_package_directory() + else: + arrow_scaling_value = dlg.ui.arrowScaling_lineEdit.text() + # if it is empty we set it to 1.0 + if not arrow_scaling_value: + arrow_scaling_value = "1.0" + else: + arrow_scaling_value = float(arrow_scaling_value) + self.signal_provider.set_custom_max_arrow( + not dlg.ui.arrowScaling_checkBox.isChecked(), + arrow_scaling_value + ) + else: self.meshcat_provider.load_model( self.signal_provider.joints_name, diff --git a/robot_log_visualizer/ui/misc/set_robot_model.ui b/robot_log_visualizer/ui/misc/set_robot_model.ui index a235eba..2b0899e 100644 --- a/robot_log_visualizer/ui/misc/set_robot_model.ui +++ b/robot_log_visualizer/ui/misc/set_robot_model.ui @@ -13,116 +13,195 @@ Dialog - - - - - QFrame::StyledPanel - - - - - - Robot Model - - - - - - - ... - - - - - - - - Ubuntu Mono - - - - - - - - - - - QFrame::StyledPanel + + + + + Qt::Vertical - - - - - - Ubuntu Mono - - - - - - - - ... - - - - - - - Package Directory - - - - - - - - - - QFrame::StyledPanel + + + 20 + 40 + - - - - - Base Frame - - - - - - - 5 - - - - - + - + Qt::Horizontal - QDialogButtonBox::Abort|QDialogButtonBox::Save + QDialogButtonBox::Close|QDialogButtonBox::Save|QDialogButtonBox::SaveAll - - - - Qt::Vertical + + + + 0 - - - 20 - 40 - - - + + + Robot + + + + + + QFrame::StyledPanel + + + + + + ... + + + + + + + Robot Model + + + + + + + + Ubuntu Mono + + + + + + + + + + + QFrame::StyledPanel + + + + + + ... + + + + + + + Package Directory + + + + + + + + Ubuntu Mono + + + + + + + + + + + QFrame::StyledPanel + + + + + + Base Frame + + + + + + + 5 + + + + + + + + + + + Miscellanea + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + Arrow scaling + + + + + + + false + + + false + + + false + + + + + + + Automatic + + + true + + + false + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + diff --git a/robot_log_visualizer/ui/misc/visualizer.ui b/robot_log_visualizer/ui/misc/visualizer.ui index c3dff71..6c87cfd 100644 --- a/robot_log_visualizer/ui/misc/visualizer.ui +++ b/robot_log_visualizer/ui/misc/visualizer.ui @@ -248,7 +248,7 @@ - + 1 @@ -553,7 +553,7 @@ - Set Robot Model + Options @@ -561,7 +561,7 @@ QWebEngineView QWidget -
PyQt5.QtWebEngineWidgets
+
QtWebEngineWidgets/QWebEngineView
1 From 7e649929b3d680d7afa1d782a1107ac525de423c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 2 Jun 2025 12:21:28 +0200 Subject: [PATCH 4/4] Format with black --- .../file_reader/signal_provider.py | 24 +++++++++++-------- robot_log_visualizer/ui/gui.py | 21 +++++++++------- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/robot_log_visualizer/file_reader/signal_provider.py b/robot_log_visualizer/file_reader/signal_provider.py index 3b5d55b..7acf2d6 100644 --- a/robot_log_visualizer/file_reader/signal_provider.py +++ b/robot_log_visualizer/file_reader/signal_provider.py @@ -96,14 +96,16 @@ def __populate_text_logging_data(self, file_object): # If len(value[text[0]].shape) == 2 then the text contains a string, otherwise it is empty # We need to manually check the shape to handle the case in which the text is empty data[key]["data"] = [ - TextLoggingMsg( - text="".join(chr(c[0]) for c in value[text[0]]), - level="".join(chr(c[0]) for c in value[level[0]]), - ) - if len(value[text[0]].shape) == 2 - else TextLoggingMsg( - text="", - level="".join(chr(c[0]) for c in value[level[0]]), + ( + TextLoggingMsg( + text="".join(chr(c[0]) for c in value[text[0]]), + level="".join(chr(c[0]) for c in value[level[0]]), + ) + if len(value[text[0]].shape) == 2 + else TextLoggingMsg( + text="", + level="".join(chr(c[0]) for c in value[level[0]]), + ) ) for text, level in zip(text_ref, level_ref) ] @@ -217,7 +219,7 @@ def robot_state_path(self): locker = QMutexLocker(self.robot_state_path_lock) value = self._robot_state_path return value - + @property def max_arrow(self): locker = QMutexLocker(self._max_arrow_mutex) @@ -399,7 +401,9 @@ def register_3d_arrow(self, key, arrow_path): data, _ = self.get_item_from_path(arrow_path) arrow = data[:, 3:] self._max_arrow_mutex.lock() - self._max_arrow = max(np.max(np.linalg.norm(arrow, axis=1)), self._max_arrow) + self._max_arrow = max( + np.max(np.linalg.norm(arrow, axis=1)), self._max_arrow + ) self._max_arrow_mutex.unlock() self._3d_arrows_path_lock.unlock() diff --git a/robot_log_visualizer/ui/gui.py b/robot_log_visualizer/ui/gui.py index efb3db5..2f7f1e0 100644 --- a/robot_log_visualizer/ui/gui.py +++ b/robot_log_visualizer/ui/gui.py @@ -103,7 +103,7 @@ def get_urdf_path(self): def get_package_directory(self): return self.ui.packageDirLineEdit.text() - + def buttonBox_on_click(self, button): self.clicked_button = button @@ -113,12 +113,12 @@ def get_clicked_button_role(self): if self.clicked_button is not None: return self.ui.buttonBox.buttonRole(self.clicked_button) return None - + def get_clicked_button_text(self): if self.clicked_button is not None: return self.clicked_button.text() return None - + def get_clicked_standard_button(self): return self.std_button @@ -131,6 +131,7 @@ def handle_arrow_scaling(self): self.ui.arrowScaling_lineEdit.setText("") self.ui.arrowScaling_lineEdit.setEnabled(True) + class About(QtWidgets.QMainWindow): def __init__(self): # call QMainWindow constructor @@ -727,8 +728,9 @@ def open_set_robot_model(self): if std_button == QtWidgets.QDialogButtonBox.SaveAll: if not self.dataset_loaded: self.meshcat_provider.model_path = dlg.get_urdf_path() - self.meshcat_provider.custom_package_dir = dlg.get_package_directory() - + self.meshcat_provider.custom_package_dir = ( + dlg.get_package_directory() + ) arrow_scaling_value = dlg.ui.arrowScaling_lineEdit.text() if not arrow_scaling_value: @@ -736,15 +738,16 @@ def open_set_robot_model(self): else: arrow_scaling_value = float(arrow_scaling_value) self.signal_provider.set_custom_max_arrow( - not dlg.ui.arrowScaling_checkBox.isChecked(), - arrow_scaling_value + not dlg.ui.arrowScaling_checkBox.isChecked(), arrow_scaling_value ) if std_button == QtWidgets.QDialogButtonBox.Save: # we need to check which tab is selected in the dlg if dlg.ui.tabWidget.currentIndex() == 0: if not self.dataset_loaded: self.meshcat_provider.model_path = dlg.get_urdf_path() - self.meshcat_provider.custom_package_dir = dlg.get_package_directory() + self.meshcat_provider.custom_package_dir = ( + dlg.get_package_directory() + ) else: arrow_scaling_value = dlg.ui.arrowScaling_lineEdit.text() # if it is empty we set it to 1.0 @@ -754,7 +757,7 @@ def open_set_robot_model(self): arrow_scaling_value = float(arrow_scaling_value) self.signal_provider.set_custom_max_arrow( not dlg.ui.arrowScaling_checkBox.isChecked(), - arrow_scaling_value + arrow_scaling_value, ) else: