commands.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import copy
4 import time
5 
6 import rospy
7 import tf
8 
9 from python_qt_binding.QtWidgets import QUndoCommand
10 
11 from constructors_geometry import FromTransformStamped
12 from frame_editor.objects import *
13 
14 
15 class Command_SelectElement(QUndoCommand):
16 
17  def __init__(self, editor, element):
18  QUndoCommand.__init__(self, "Select")
19  self.editor = editor
20 
21  self.new_element = element
22  self.old_element = editor.active_frame
23 
24  def redo(self):
25  self.editor.active_frame = self.new_element
26  self.editor.add_undo_level(2, [self.new_element, self.old_element])
27 
28  def undo(self):
29  self.editor.active_frame = self.old_element
30  self.editor.add_undo_level(2, [self.new_element, self.old_element])
31 
32 
33 class Command_AddElement(QUndoCommand):
34 
35  def __init__(self, editor, element):
36  QUndoCommand.__init__(self, "Add")
37  self.editor = editor
38 
39  self.element = element
40 
41  def redo(self):
42  self.editor.frames[self.element.name] = self.element
43  self.element.hidden = False
44  self.editor.add_undo_level(1, [self.element])
45 
46  def undo(self):
47  del self.editor.frames[self.element.name]
48  self.element.hidden = True
49  self.editor.add_undo_level(1, [self.element])
50 
51 
52 class Command_RemoveElement(QUndoCommand):
53 
54  def __init__(self, editor, element):
55  QUndoCommand.__init__(self, "Remove")
56  self.editor = editor
57 
58  self.element = element
59 
60  if editor.active_frame is element:
61  self.was_active = True
62  else:
63  self.was_active = False
64 
65  def redo(self):
66  if self.was_active:
67  self.editor.active_frame = None
68  self.editor.add_undo_level(2)
69 
70  del self.editor.frames[self.element.name]
71  self.element.hidden = True
72  self.editor.add_undo_level(1, [self.element])
73 
74  def undo(self):
75  self.editor.frames[self.element.name] = self.element
76  self.element.hidden = False
77  self.editor.add_undo_level(1, [self.element])
78 
79  if self.was_active:
80  self.editor.active_frame = self.element
81  self.editor.add_undo_level(2)
82 
83 
84 class Command_ClearAll(QUndoCommand):
85 
86  def __init__(self, editor):
87  QUndoCommand.__init__(self, "Clear all")
88  self.editor = editor
89 
90  self.elements = editor.frames
91  self.active_element = editor.active_frame
92 
93  def redo(self):
94  self.editor.active_frame = None
95  self.editor.frames = {}
96  self.editor.add_undo_level(1+2, self.elements.values())
97 
98  def undo(self):
99  self.editor.active_frame = self.active_element
100  self.editor.frames = self.elements
101  self.editor.add_undo_level(1+2, self.elements.values())
102 
103 
104 class Command_AlignElement(QUndoCommand):
105 
106  def __init__(self, editor, element, source_name, mode):
107  QUndoCommand.__init__(self, "Align")
108  self.editor = editor
109 
110  self.element = element
111 
112  self.old_position = element.position
113  self.old_orientation = element.orientation
114 
115  ## New Pose ##
116 
117  position, orientation = FromTransformStamped(
118  element.tf_buffer.lookup_transform(
119  element.parent, source_name, rospy.Time(0)))
120 
121  ## Position
122  pos = list(element.position)
123  if "x" in mode:
124  pos[0] = position[0]
125  if "y" in mode:
126  pos[1] = position[1]
127  if "z" in mode:
128  pos[2] = position[2]
129  self.new_position = tuple(pos)
130 
131  ## Orientation
132  rpy = list(tf.transformations.euler_from_quaternion(element.orientation))
133  rpy_new = tf.transformations.euler_from_quaternion(orientation)
134  if "a" in mode:
135  rpy[0] = rpy_new[0]
136  if "b" in mode:
137  rpy[1] = rpy_new[1]
138  if "c" in mode:
139  rpy[2] = rpy_new[2]
140 
141  if "a" in mode or "b" in mode or "c" in mode:
142  self.new_orientation = tf.transformations.quaternion_from_euler(*rpy)
143  else:
144  self.new_orientation = self.old_orientation
145 
146  def redo(self):
147  self.element.position = self.new_position
148  self.element.orientation = self.new_orientation
149  self.editor.add_undo_level(4, [self.element])
150 
151  def undo(self):
152  self.element.position = self.old_position
153  self.element.orientation = self.old_orientation
154  self.editor.add_undo_level(4, [self.element])
155 
156 
157 class Command_CopyElement(QUndoCommand):
158  '''Copys a source frame's transformation and sets a new parent
159  '''
160 
161  def __init__(self, editor, new_name, source_name, parent_name):
162  QUndoCommand.__init__(self, "Rebase")
163  self.editor = editor
164 
165  if source_name in self.editor.frames:
166  element = copy.deepcopy(self.editor.frames[source_name])
167  element.name = new_name
168  else:
169  element = Frame(new_name, parent=parent_name)
170  element.parent = parent_name
171 
172  # Pose
173  position, orientation = FromTransformStamped(
174  element.tf_buffer.lookup_transform(
175  parent_name, source_name, rospy.Time(0)))
176  element.position = position
177  element.orientation = orientation
178 
179  self.element = element
180 
181  def redo(self):
182  self.editor.frames[self.element.name] = self.element
183  self.element.hidden = False
184  self.editor.add_undo_level(1, [self.element])
185 
186  def undo(self):
187  del self.editor.frames[self.element.name]
188  self.element.hidden = True
189  self.editor.add_undo_level(1, [self.element])
190 
191 
192 
193 class Command_RebaseElement(QUndoCommand):
194  '''Copys a source frame's transformation and sets a new parent
195  '''
196 
197  def __init__(self, editor, element, source_name, new_parent):
198  QUndoCommand.__init__(self, "Rebase")
199  self.editor = editor
200 
201  self.element = element
202 
203  self.old_position = element.position
204  self.old_orientation = element.orientation
205 
206  self.new_parent = new_parent
207  self.old_parent = element.parent
208 
209  # New Pose
210  self.new_position, self.new_orientation = FromTransformStamped(
211  element.tf_buffer.lookup_transform(
212  new_parent, source_name, rospy.Time(0)))
213 
214 
215  def redo(self):
216  self.element.position = self.new_position
217  self.element.orientation = self.new_orientation
218  self.element.parent = self.new_parent
219  self.editor.add_undo_level(4, [self.element])
220 
221  def undo(self):
222  self.element.position = self.old_position
223  self.element.orientation = self.old_orientation
224  self.element.parent = self.old_parent
225  self.editor.add_undo_level(4, [self.element])
226 
227 
228 class Command_SetPose(QUndoCommand):
229 
230  def __init__(self, editor, element, position, orientation):
231  QUndoCommand.__init__(self, "Position")
232  self.editor = editor
233 
234  self.element = element
235 
236  self.time = time.time()
237 
238  self.new_position = position
239  self.new_orientation = orientation
240  self.old_position = element.position
241  self.old_orientation = element.orientation
242 
243  def redo(self):
244  self.element.position = self.new_position
245  self.element.orientation = self.new_orientation
246  self.editor.add_undo_level(4, [self.element])
247 
248  def undo(self):
249  self.element.position = self.old_position
250  self.element.orientation = self.old_orientation
251  self.editor.add_undo_level(4, [self.element])
252 
253  def id(self):
254  return 1
255 
256  def mergeWith(self, command):
257  if self.id() != command.id():
258  return False
259  if self.element is not command.element:
260  return False
261  if time.time() - self.time > 1.0:
262  return False # don't merge if too old
263 
264  ## Merge
265  self.time = time.time()
266  self.new_position = command.new_position
267  self.new_orientation = command.new_orientation
268  return True
269 
270 
271 class Command_SetPosition(QUndoCommand):
272 
273  def __init__(self, editor, element, position):
274  QUndoCommand.__init__(self, "Position")
275  self.editor = editor
276 
277  self.element = element
278 
279  self.new_position = position
280  self.old_position = element.position
281 
282  def redo(self):
283  self.element.position = self.new_position
284  self.editor.add_undo_level(4, [self.element])
285 
286  def undo(self):
287  self.element.position = self.old_position
288  self.editor.add_undo_level(4, [self.element])
289 
290 
291 class Command_SetOrientation(QUndoCommand):
292 
293  def __init__(self, editor, element, orientation):
294  QUndoCommand.__init__(self, "Orientation")
295  self.editor = editor
296 
297  self.element = element
298 
299  self.new_orientation = orientation
300  self.old_orientation = element.orientation
301 
302  def redo(self):
303  self.element.orientation = self.new_orientation
304  self.editor.add_undo_level(4, [self.element])
305 
306  def undo(self):
307  self.element.orientation = self.old_orientation
308  self.editor.add_undo_level(4, [self.element])
309 
310 
311 class Command_SetValue(QUndoCommand):
312 
313  def __init__(self, editor, element, symbol, value):
314  QUndoCommand.__init__(self, "Value")
315  self.editor = editor
316 
317  self.element = element
318  self.symbol = symbol
319 
320  self.new_value = value
321  self.old_value = element.value(symbol)
322 
323  def redo(self):
324  self.element.set_value(self.symbol, self.new_value)
325  self.editor.add_undo_level(4, [self.element])
326 
327  def undo(self):
328  self.element.set_value(self.symbol, self.old_value)
329  self.editor.add_undo_level(4, [self.element])
330 
331 
332 class Command_SetParent(QUndoCommand):
333 
334  def __init__(self, editor, element, parent_name, keep_absolute=True):
335  QUndoCommand.__init__(self, "Parent")
336  self.editor = editor
337 
338  self.element = element
339  self.keep_absolute = keep_absolute
340 
341  self.new_parent_name = parent_name
342  self.old_parent_name = element.parent
343 
344  if self.keep_absolute:
345  position, orientation = FromTransformStamped(
346  element.tf_buffer.lookup_transform(
347  parent_name, element.name, rospy.Time(0)))
348  self.new_position = position
349  self.new_orientation = orientation
350 
351  self.old_position = element.position
352  self.old_orientation = element.orientation
353 
354  def redo(self):
355  self.element.parent = self.new_parent_name
356 
357  if self.keep_absolute:
358  self.element.position = self.new_position
359  self.element.orientation = self.new_orientation
360 
361  self.editor.add_undo_level(4, [self.element])
362 
363  def undo(self):
364  self.element.parent = self.old_parent_name
365 
366  if self.keep_absolute:
367  self.element.position = self.old_position
368  self.element.orientation = self.old_orientation
369 
370  self.editor.add_undo_level(4, [self.element])
371 
372 
373 class Command_SetStyle(QUndoCommand):
374 
375  def __init__(self, editor, element, style):
376  QUndoCommand.__init__(self, "Style")
377  self.editor = editor
378 
379  self.old_element = element
380 
381  if style == "plane":
382  self.new_element = Object_Plane(element.name, element.position, element.orientation, element.parent)
383  elif style == "cube":
384  self.new_element = Object_Cube(element.name, element.position, element.orientation, element.parent)
385  elif style == "sphere":
386  self.new_element = Object_Sphere(element.name, element.position, element.orientation, element.parent)
387  elif style == "axis":
388  self.new_element = Object_Axis(element.name, element.position, element.orientation, element.parent)
389  elif style == "mesh":
390  self.new_element = Object_Mesh(element.name, element.position, element.orientation, element.parent)
391  else:
392  self.new_element = Frame(element.name, element.position, element.orientation, element.parent)
393 
394  if editor.active_frame is element:
395  self.was_active = True
396  else:
397  self.was_active = False
398 
399  def redo(self):
400  del self.editor.frames[self.old_element.name]
401  self.old_element.hidden = True
402 
403  self.editor.frames[self.new_element.name] = self.new_element
404  self.new_element.hidden = False
405 
406  self.editor.add_undo_level(1+4, [self.new_element, self.old_element])
407 
408  if self.was_active:
409  self.editor.active_frame = self.new_element
410  self.editor.add_undo_level(2)
411 
412  def undo(self):
413  del self.editor.frames[self.new_element.name]
414  self.new_element.hidden = True
415 
416  self.editor.frames[self.old_element.name] = self.old_element
417  self.old_element.hidden = False
418 
419  self.editor.add_undo_level(1+4, [self.new_element, self.old_element])
420 
421  if self.was_active:
422  self.editor.active_frame = self.old_element
423  self.editor.add_undo_level(2)
424 
425 
426 class Command_SetStyleColor(QUndoCommand):
427 
428  def __init__(self, editor, element, color_rgba):
429  QUndoCommand.__init__(self, "Style Color")
430  self.editor = editor
431 
432  self.element = element
433  self.old_color = element.color
434  self.new_color = color_rgba
435 
436  def redo(self):
437  self.element.set_color(self.new_color)
438  self.editor.add_undo_level(4, [self.element])
439 
440  def undo(self):
441  self.element.set_color(self.old_color)
442  self.editor.add_undo_level(4, [self.element])
443 
444 
445 class Command_SetGeometry(QUndoCommand):
446 
447  def __init__(self, editor, element, parameter, value):
448  QUndoCommand.__init__(self, "Style Geometry")
449  self.editor = editor
450 
451  self.element = element
452  self.parameter = parameter
453  self.old_value = getattr(element, parameter)
454  self.new_value = value
455 
456  def redo(self):
457  setattr(self.element, self.parameter, self.new_value)
458  self.element.update_marker()
459  self.editor.add_undo_level(4, [self.element])
460 
461 
462  def undo(self):
463  setattr(self.element, self.parameter, self.old_value)
464  self.element.update_marker()
465  self.editor.add_undo_level(4, [self.element])
466 
467 # eof
def __init__(self, editor, element, symbol, value)
Definition: commands.py:313
def __init__(self, editor, element)
Definition: commands.py:35
def __init__(self, editor, element, source_name, mode)
Definition: commands.py:106
def __init__(self, editor, element, source_name, new_parent)
Definition: commands.py:197
def __init__(self, editor, new_name, source_name, parent_name)
Definition: commands.py:161
def __init__(self, editor, element, orientation)
Definition: commands.py:293
def __init__(self, editor, element, style)
Definition: commands.py:375
def __init__(self, editor, element, color_rgba)
Definition: commands.py:428
def __init__(self, editor, element, position)
Definition: commands.py:273
def __init__(self, editor, element, parent_name, keep_absolute=True)
Definition: commands.py:334
def __init__(self, editor, element, position, orientation)
Definition: commands.py:230
def __init__(self, editor, element)
Definition: commands.py:17
def __init__(self, editor, element)
Definition: commands.py:54
def FromTransformStamped(msg)
TransformStamped ##.
def __init__(self, editor, element, parameter, value)
Definition: commands.py:447


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