rqt_editor.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import os
4 import math
5 
6 import rospy
7 import rospkg
8 import tf
9 import actionlib
10 
11 from qt_gui_py_common.worker_thread import WorkerThread
12 
13 from python_qt_binding import loadUi, QtCore, QtWidgets
14 from python_qt_binding.QtWidgets import QWidget
15 from python_qt_binding.QtCore import Slot
16 
17 from frame_editor.editor import Frame, FrameEditor
18 from frame_editor.commands import *
20 
21 from project_plugin import ProjectPlugin
22 
23 from frame_editor.interface import Interface
24 
25 ## Views
26 from frame_editor.interface_gui import FrameEditor_StyleWidget
27 
28 
29 class FrameEditorGUI(ProjectPlugin, Interface):
30 
31  signal_update = QtCore.Signal(int)
32 
33  def __init__(self, context):
34  super(FrameEditorGUI, self).__init__(context)
35 
36  self.setObjectName('FrameEditorGUI')
37 
38  self.file_type = "YAML files(*.yaml)"
39 
40  self.editor.parse_args(context.argv())
41 
42  # Update filename display
43  self.update_current_filename()
44 
45 
46  ## Update thread ##
47 
48  self._update_thread.start()
49  self.update_all(3)
50 
51 
52  def create_editor(self):
53  editor = FrameEditor()
54 
55  editor.observers.append(self)
56 
57  self.signal_update.connect(self.update_all)
58 
59  self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
60 
61  self.old_frame_list = []
62  self.old_selected = ""
63 
64  return editor
65 
66 
67  def create_main_widget(self):
68 
69  ## Main widget
70  widget = QWidget()
71  ui_file = os.path.join(rospkg.RosPack().get_path('frame_editor'), 'src/frame_editor', 'FrameEditorGUI.ui')
72  loadUi(ui_file, widget)
73  widget.setObjectName('FrameEditorGUIUi')
74 
75  #if context.serial_number() > 1:
76  # widget.setWindowTitle(widget.windowTitle() + (' (%d)' % context.serial_number()))
77  widget.setWindowTitle("frame editor")
78 
79 
80  ## Undo View
81  #widget.undo_frame.layout().addWidget(QtWidgets.QUndoView(self.editor.undo_stack))
82 
83 
84  ## Views
85  self.editor.init_views()
87 
88  widget.style_frame.layout().addWidget(self.interface_style.get_widget())
89 
90  ## Connections ##
91 
92  widget.btn_add.clicked.connect(self.btn_add_clicked)
93  widget.btn_delete.clicked.connect(self.btn_delete_clicked)
94  widget.btn_duplicate.clicked.connect(self.btn_duplicate_clicked)
95  widget.list_frames.currentTextChanged.connect(self.selected_frame_changed)
96  widget.btn_refresh.clicked.connect(self.update_tf_list)
97 
98  widget.btn_set_parent_rel.clicked.connect(self.btn_set_parent_rel_clicked)
99  widget.btn_set_parent_abs.clicked.connect(self.btn_set_parent_abs_clicked)
100  widget.btn_set_pose.clicked.connect(self.btn_set_pose_clicked)
101  widget.btn_set_position.clicked.connect(self.btn_set_position_clicked)
102  widget.btn_set_orientation.clicked.connect(self.btn_set_orientation_clicked)
103  widget.btn_set_x.clicked.connect(self.btn_set_x_clicked)
104  widget.btn_set_y.clicked.connect(self.btn_set_y_clicked)
105  widget.btn_set_z.clicked.connect(self.btn_set_z_clicked)
106  widget.btn_set_a.clicked.connect(self.btn_set_a_clicked)
107  widget.btn_set_b.clicked.connect(self.btn_set_b_clicked)
108  widget.btn_set_c.clicked.connect(self.btn_set_c_clicked)
109 
110  widget.btn_reset_position_rel.clicked.connect(self.btn_reset_position_rel_clicked)
111  widget.btn_reset_position_abs.clicked.connect(self.btn_reset_position_abs_clicked)
112  widget.btn_reset_orientation_rel.clicked.connect(self.btn_reset_orientation_rel_clicked)
113  widget.btn_reset_orientation_abs.clicked.connect(self.btn_reset_orientation_abs_clicked)
114 
115  widget.txt_x.editingFinished.connect(self.x_valueChanged)
116  widget.txt_y.editingFinished.connect(self.y_valueChanged)
117  widget.txt_z.editingFinished.connect(self.z_valueChanged)
118  widget.txt_a.editingFinished.connect(self.a_valueChanged)
119  widget.txt_b.editingFinished.connect(self.b_valueChanged)
120  widget.txt_c.editingFinished.connect(self.c_valueChanged)
121 
122  widget.btn_rad.toggled.connect(self.update_fields)
123 
124  widget.combo_style.currentIndexChanged.connect(self.frame_style_changed)
125 
126  return widget
127 
128 
130  self.editor.run()
131 
132  @Slot()
133  def _update_finished(self):
134  print("> Shutting down")
135 
136 
137  def update(self, editor, level, elements):
138  self.signal_update.emit(level)
139 
140 
141  @Slot(int)
142  def update_all(self, level):
143  ## Update list widgets
144  if level & 1:
145  self.update_frame_list()
146  self.update_tf_list()
147  self.update_current_filename()
148 
149  ## Update the currently selected frame
150  if level & 2:
151  self.update_active_frame()
152 
153  ## Update only text fields, spin boxes,...
154  if level & 4:
155  self.update_fields()
156 
157 
158  @Slot()
159  def update_tf_list(self):
160  self.widget.list_tf.clear()
161  self.widget.list_tf.addItems(
162  sorted(self.editor.all_frame_ids(include_temp=False)))
163 
164  def update_frame_list(self):
165  new_list = self.editor.frames.keys()
166 
167  ## Add missing
168  items = []
169  for item in new_list:
170  if item not in self.old_frame_list:
171  items.append(item)
172  self.widget.list_frames.addItems(items)
173 
174  ## Delete removed
175  for item in self.old_frame_list:
176  if item not in new_list:
177  if self.widget.list_frames.currentItem() and item == self.widget.list_frames.currentItem().text():
178  self.widget.list_frames.setCurrentItem(None)
179  found = self.widget.list_frames.findItems(item, QtCore.Qt.MatchExactly)
180  self.widget.list_frames.takeItem(self.widget.list_frames.row(found[0]))
181 
182  self.widget.list_frames.sortItems()
183 
184  self.old_frame_list = new_list
185 
186 
188  if not self.editor.active_frame:
189  self.old_selected = ""
190  self.widget.list_frames.setCurrentItem(None)
191  self.widget.box_edit.setEnabled(False)
192  return # deselect and quit
193 
194  self.widget.box_edit.setEnabled(True)
195 
196  name = self.editor.active_frame.name
197  if name == self.old_selected:
198  return # no change
199 
200  ## Select item in list
201  items = self.widget.list_frames.findItems(name, QtCore.Qt.MatchExactly)
202  self.widget.list_frames.setCurrentItem(items[0])
203 
204  self.update_fields()
205 
206  self.old_selected = name
207 
208 
209  @Slot()
210  def update_fields(self):
211 
212  f = self.editor.active_frame
213  if not f:
214  return
215 
216  w = self.widget
217 
218  w.txt_name.setText(f.name)
219  w.txt_parent.setText(f.parent)
220 
221  ## Relative
222  w.txt_x.setValue(f.position[0])
223  w.txt_y.setValue(f.position[1])
224  w.txt_z.setValue(f.position[2])
225 
226  rot = tf.transformations.euler_from_quaternion(f.orientation)
227  if self.widget.btn_deg.isChecked():
228  rot = (180.0*rot[0]/math.pi, 180.0*rot[1]/math.pi, 180.0*rot[2]/math.pi)
229 
230  w.txt_a.setValue(rot[0])
231  w.txt_b.setValue(rot[1])
232  w.txt_c.setValue(rot[2])
233 
234  txt_abs_pos = (w.txt_abs_x, w.txt_abs_y, w.txt_abs_z)
235  txt_abs_rot = (w.txt_abs_a, w.txt_abs_b, w.txt_abs_c)
236 
237  ## Absolute
238  try:
239  position, orientation = FromTransformStamped(
240  f.tf_buffer.lookup_transform('world', f.name, rospy.Time(0)))
241  for txt, p in zip(txt_abs_pos, position):
242  txt.setEnabled(True)
243  txt.setValue(p)
244  rot = tf.transformations.euler_from_quaternion(orientation)
245  if self.widget.btn_deg.isChecked():
246  rot = map(math.degrees, rot)
247  for txt, r in zip(txt_abs_rot, rot):
248  txt.setEnabled(True)
249  txt.setValue(r)
250  except:
251  for txt in txt_abs_rot + txt_abs_pos:
252  txt.setEnabled(False)
253 
254  ## Style
255  self.widget.combo_style.setCurrentIndex(self.widget.combo_style.findText(f.style))
256 
257 
258  @Slot(str)
259  def selected_frame_changed(self, name):
260  if name == "":
261  return
262 
263  if not self.editor.active_frame or (self.editor.active_frame.name != name):
264  self.editor.command(Command_SelectElement(self.editor, self.editor.frames[name]))
265 
266 
267  ## BUTTONS ##
268 
269  def write_file(self, file_name):
270  return self.editor.save_file(file_name)
271 
272 
273  @Slot()
274  def clear_all(self):
275  self.editor.command(Command_ClearAll(self.editor))
276 
277  @Slot(bool)
278  def btn_add_clicked(self, checked):
279  # Get a unique frame name
280  existing_frames = set(self.editor.all_frame_ids())
281 
282  name, ok = QtWidgets.QInputDialog.getText(self.widget, "Add New Frame", "Name:", QtWidgets.QLineEdit.Normal, "my_frame");
283 
284  while ok and name in existing_frames:
285  name, ok = QtWidgets.QInputDialog.getText(self.widget, "Add New Frame", "Name (must be unique):", QtWidgets.QLineEdit.Normal, "my_frame")
286  if not ok:
287  return
288 
289  if not existing_frames:
290  available_parents = ["world"]
291  else:
292  available_parents = self.editor.all_frame_ids(include_temp=False)
293  parent, ok = QtWidgets.QInputDialog.getItem(self.widget, "Add New Frame", "Parent Name:", sorted(available_parents))
294 
295 
296  if not ok or parent == "":
297  return
298 
299  self.editor.command(Command_AddElement(self.editor, Frame(name, parent=parent)))
300 
301 
302 
303  @Slot(bool)
304  def btn_duplicate_clicked(self, checked):
305  item = self.widget.list_frames.currentItem()
306  if not item:
307  return
308  source_name = item.text()
309  parent_name = self.editor.frames[source_name].parent
310 
311  # Get a unique frame name
312  existing_frames = set(self.editor.all_frame_ids())
313 
314  name, ok = QtWidgets.QInputDialog.getText(self.widget, "Duplicate Frame", "Name:", QtWidgets.QLineEdit.Normal, source_name);
315 
316  while ok and name in existing_frames:
317  name, ok = QtWidgets.QInputDialog.getText(self.widget, "Duplicate Frame", "Name (must be unique):", QtWidgets.QLineEdit.Normal, source_name)
318  if not ok:
319  return
320 
321  self.editor.command(Command_CopyElement(self.editor, name, source_name, parent_name))
322 
323 
324 
325  @Slot(bool)
326  def btn_delete_clicked(self, checked):
327  item = self.widget.list_frames.currentItem()
328  if not item:
329  return
330  self.editor.command(Command_RemoveElement(self.editor, self.editor.frames[item.text()]))
331 
332 
333  ## PARENTING ##
334 
335  @Slot(bool)
336  def btn_set_parent_rel_clicked(self, checked):
337  self.set_parent(False)
338 
339  @Slot(bool)
340  def btn_set_parent_abs_clicked(self, checked):
341  self.set_parent(True)
342 
343  def set_parent(self, keep_absolute):
344  parent = self.widget.list_tf.currentItem()
345  if not parent:
346  return # none selected
347 
348  if parent.text() == self.editor.active_frame.name:
349  return # you can't be your own parent
350 
351  self.editor.command(Command_SetParent(self.editor, self.editor.active_frame, parent.text(), keep_absolute))
352 
353 
354  ## SET BUTTONS ##
355 
356  @Slot(bool)
357  def btn_set_pose_clicked(self, checked):
358  self.set_pose(["x", "y", "z", "a", "b", "c"])
359 
360  @Slot(bool)
361  def btn_set_position_clicked(self, checked):
362  self.set_pose(["x", "y", "z"])
363 
364  @Slot(bool)
365  def btn_set_orientation_clicked(self, checked):
366  self.set_pose(["a", "b", "c"])
367 
368  @Slot(bool)
369  def btn_set_x_clicked(self, checked):
370  self.set_pose(["x"])
371  @Slot(bool)
372  def btn_set_y_clicked(self, checked):
373  self.set_pose(["y"])
374  @Slot(bool)
375  def btn_set_z_clicked(self, checked):
376  self.set_pose(["z"])
377  @Slot(bool)
378  def btn_set_a_clicked(self, checked):
379  self.set_pose(["a"])
380  @Slot(bool)
381  def btn_set_b_clicked(self, checked):
382  self.set_pose(["b"])
383  @Slot(bool)
384  def btn_set_c_clicked(self, checked):
385  self.set_pose(["c"])
386 
387  def set_pose(self, mode):
388  source = self.widget.list_tf.currentItem()
389  if not source:
390  return # none selected
391 
392  frame = self.editor.active_frame
393  self.editor.command(Command_AlignElement(self.editor, frame, source.text(), mode))
394 
395 
396  ## RESET BUTTONS ##
397 
398  @Slot(bool)
399  def btn_reset_position_rel_clicked(self, checked):
400  self.editor.command(Command_SetPosition(self.editor, self.editor.active_frame, (0, 0, 0)))
401 
402  @Slot(bool)
403  def btn_reset_position_abs_clicked(self, checked):
404  position, orientation = FromTransformStamped(
405  self.editor.active_frame.tf_buffer.lookup_transform(
406  self.editor.active_frame.parent, "world", rospy.Time(0)))
407  self.editor.command(Command_SetPosition(self.editor, self.editor.active_frame, position))
408 
409  @Slot(bool)
411  self.editor.command(Command_SetOrientation(self.editor, self.editor.active_frame, (0, 0, 0, 1)))
412 
413  @Slot(bool)
415  position, orientation = FromTransformStamped(
416  self.editor.active_frame.listener.lookupTransform(
417  self.editor.active_frame.parent, "world", rospy.Time(0)))
418  self.editor.command(Command_SetOrientation(self.editor, self.editor.active_frame, orientation))
419 
420 
421  ## Spin Boxes ##
422 
423  def set_value(self, widget, symbol):
424  frame = self.editor.active_frame
425  value = widget.value()
426 
427  ## Deg to rad
428  if self.widget.btn_deg.isChecked() and symbol in ['a', 'b', 'c']:
429  value = value * math.pi / 180.0
430 
431  if frame.value(symbol) != value:
432  self.editor.command(Command_SetValue(self.editor, self.editor.active_frame, symbol, value))
433 
434  @Slot()
435  def x_valueChanged(self):
436  self.set_value(self.widget.txt_x, 'x')
437  @Slot()
438  def y_valueChanged(self):
439  self.set_value(self.widget.txt_y, 'y')
440  @Slot()
441  def z_valueChanged(self):
442  self.set_value(self.widget.txt_z, 'z')
443  @Slot()
444  def a_valueChanged(self):
445  self.set_value(self.widget.txt_a, 'a')
446  @Slot()
447  def b_valueChanged(self):
448  self.set_value(self.widget.txt_b, 'b')
449  @Slot()
450  def c_valueChanged(self):
451  self.set_value(self.widget.txt_c, 'c')
452 
453 
454  ## FRAME STYLE ##
455 
456  @Slot(int)
457  def frame_style_changed(self, id):
458  style = self.widget.combo_style.currentText().lower()
459  if self.editor.active_frame.style != style:
460  self.editor.command(Command_SetStyle(self.editor, self.editor.active_frame, style))
461 
462 
463  ## PLUGIN ##
464 
465  def shutdown_plugin(self):
466  super(FrameEditorGUI, self).shutdown_plugin()
467  self._update_thread.kill()
468 
469 # eof
def btn_set_parent_abs_clicked(self, checked)
Definition: rqt_editor.py:340
def btn_reset_orientation_rel_clicked(self, checked)
Definition: rqt_editor.py:410
def btn_set_pose_clicked(self, checked)
SET BUTTONS ##.
Definition: rqt_editor.py:357
def btn_set_position_clicked(self, checked)
Definition: rqt_editor.py:361
def btn_set_parent_rel_clicked(self, checked)
PARENTING ##.
Definition: rqt_editor.py:336
def btn_reset_position_abs_clicked(self, checked)
Definition: rqt_editor.py:403
def btn_reset_position_rel_clicked(self, checked)
RESET BUTTONS ##.
Definition: rqt_editor.py:399
def shutdown_plugin(self)
PLUGIN ##.
Definition: rqt_editor.py:465
def btn_duplicate_clicked(self, checked)
Definition: rqt_editor.py:304
def btn_set_orientation_clicked(self, checked)
Definition: rqt_editor.py:365
def set_value(self, widget, symbol)
Spin Boxes ##.
Definition: rqt_editor.py:423
def update(self, editor, level, elements)
Definition: rqt_editor.py:137
old_selected
Select item in list.
Definition: rqt_editor.py:62
def write_file(self, file_name)
BUTTONS ##.
Definition: rqt_editor.py:269
def FromTransformStamped(msg)
TransformStamped ##.
def set_parent(self, keep_absolute)
Definition: rqt_editor.py:343
def frame_style_changed(self, id)
FRAME STYLE ##.
Definition: rqt_editor.py:457
def btn_reset_orientation_abs_clicked(self, checked)
Definition: rqt_editor.py:414


frame_editor
Author(s): ipa-lth , ipa-frn
autogenerated on Wed Oct 21 2020 03:36:00