Merge branch 'ros2' of github.com:RVMI/skiros2 into ros2

RVMI · Feb 28, 2024 · 50a83df · 50a83df
2 parents 491d4aa + 915b167
commit 50a83df
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/skiros2/launch/skiros2.launch.py b/skiros2/launch/skiros2.launch.py
@@ -118,5 +118,5 @@ def generate_launch_description():
  robot_ontology_prefix_arg,
  static_tf,
  wm,
- # gui,
+ gui,
  skill_mgr])
diff --git a/skiros2_gui/skiros2_gui/core/skiros_widget.py b/skiros2_gui/skiros2_gui/core/skiros_widget.py
@@ -438,6 +438,7 @@ class SkirosWidget(QWidget, SkirosInteractiveMarkers):
  def __init__(self, node, initial_topics=None, start_paused=False):
  super(SkirosWidget, self).__init__()
  self._node = node
+ self._marker_update = None
  self.setObjectName('SkirosWidget')
 
  _, self.package_path = get_resource('packages', 'skiros2_gui')
@@ -610,6 +611,8 @@ def refresh_timer_cb(self):
  # Update WM
  if self._wmi.is_connected() and self._snapshot_id == "":
  self.create_wm_tree()
+ # Process marker feedback
+ self._process_marker_feedback()
  # Update robot BT rate
  if self._sli.agents:
  robot_info = ""
@@ -818,9 +821,14 @@ def on_wm_update(self, data):
 
  def on_marker_feedback(self, feedback):
  if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
+ self._marker_update = feedback
+
+ def _process_marker_feedback(self):
+ if self._marker_update is not None:
  with self._wm_mutex:
- elem = self._wmi.get_element(feedback.marker_name)
- elem.setData(":PoseStampedMsg", feedback)
+ elem = self._wmi.get_element(self._marker_update.marker_name)
+ elem.setData(":PoseStampedMsg", self._marker_update)
+ self._marker_update = None
  self._wmi.update_element_properties(elem)
 
  @Slot()