00001
00002
00003 import copy
00004 import time
00005
00006 import rospy
00007 import tf
00008
00009 from python_qt_binding.QtGui import QUndoCommand
00010
00011 from constructors_geometry import FromTransformStamped
00012 from frame_editor.objects import *
00013
00014
00015 class Command_SelectElement(QUndoCommand):
00016
00017 def __init__(self, editor, element):
00018 QUndoCommand.__init__(self, "Select")
00019 self.editor = editor
00020
00021 self.new_element = element
00022 self.old_element = editor.active_frame
00023
00024 def redo(self):
00025 self.editor.active_frame = self.new_element
00026 self.editor.add_undo_level(2, [self.new_element, self.old_element])
00027
00028 def undo(self):
00029 self.editor.active_frame = self.old_element
00030 self.editor.add_undo_level(2, [self.new_element, self.old_element])
00031
00032
00033 class Command_AddElement(QUndoCommand):
00034
00035 def __init__(self, editor, element):
00036 QUndoCommand.__init__(self, "Add")
00037 self.editor = editor
00038
00039 self.element = element
00040
00041 def redo(self):
00042 self.editor.frames[self.element.name] = self.element
00043 self.element.hidden = False
00044 self.editor.add_undo_level(1, [self.element])
00045
00046 def undo(self):
00047 del self.editor.frames[self.element.name]
00048 self.element.hidden = True
00049 self.editor.add_undo_level(1, [self.element])
00050
00051
00052 class Command_RemoveElement(QUndoCommand):
00053
00054 def __init__(self, editor, element):
00055 QUndoCommand.__init__(self, "Remove")
00056 self.editor = editor
00057
00058 self.element = element
00059
00060 if editor.active_frame is element:
00061 self.was_active = True
00062 else:
00063 self.was_active = False
00064
00065 def redo(self):
00066 if self.was_active:
00067 self.editor.active_frame = None
00068 self.editor.add_undo_level(2)
00069
00070 del self.editor.frames[self.element.name]
00071 self.element.tf_buffer.clear()
00072 self.element.hidden = True
00073 self.editor.add_undo_level(1, [self.element])
00074
00075 def undo(self):
00076 self.editor.frames[self.element.name] = self.element
00077 self.element.hidden = False
00078 self.editor.add_undo_level(1, [self.element])
00079
00080 if self.was_active:
00081 self.editor.active_frame = self.element
00082 self.editor.add_undo_level(2)
00083
00084
00085 class Command_ClearAll(QUndoCommand):
00086
00087 def __init__(self, editor):
00088 QUndoCommand.__init__(self, "Clear all")
00089 self.editor = editor
00090
00091 self.elements = editor.frames
00092 self.active_element = editor.active_frame
00093
00094 def redo(self):
00095 self.editor.active_frame = None
00096 self.editor.frames = {}
00097 self.editor.add_undo_level(1+2, self.elements.values())
00098
00099 def undo(self):
00100 self.editor.active_frame = self.active_element
00101 self.editor.frames = self.elements
00102 self.editor.add_undo_level(1+2, self.elements.values())
00103
00104
00105 class Command_AlignElement(QUndoCommand):
00106
00107 def __init__(self, editor, element, source_name, mode):
00108 QUndoCommand.__init__(self, "Align")
00109 self.editor = editor
00110
00111 self.element = element
00112
00113 self.old_position = element.position
00114 self.old_orientation = element.orientation
00115
00116
00117
00118 position, orientation = FromTransformStamped(
00119 element.tf_buffer.lookup_transform(
00120 element.parent, source_name, rospy.Time(0)))
00121
00122
00123 pos = list(element.position)
00124 if "x" in mode:
00125 pos[0] = position[0]
00126 if "y" in mode:
00127 pos[1] = position[1]
00128 if "z" in mode:
00129 pos[2] = position[2]
00130 self.new_position = tuple(pos)
00131
00132
00133 rpy = list(tf.transformations.euler_from_quaternion(element.orientation))
00134 rpy_new = tf.transformations.euler_from_quaternion(orientation)
00135 if "a" in mode:
00136 rpy[0] = rpy_new[0]
00137 if "b" in mode:
00138 rpy[1] = rpy_new[1]
00139 if "c" in mode:
00140 rpy[2] = rpy_new[2]
00141
00142 if "a" in mode or "b" in mode or "c" in mode:
00143 self.new_orientation = tf.transformations.quaternion_from_euler(*rpy)
00144 else:
00145 self.new_orientation = self.old_orientation
00146
00147 def redo(self):
00148 self.element.position = self.new_position
00149 self.element.orientation = self.new_orientation
00150 self.editor.add_undo_level(4, [self.element])
00151
00152 def undo(self):
00153 self.element.position = self.old_position
00154 self.element.orientation = self.old_orientation
00155 self.editor.add_undo_level(4, [self.element])
00156
00157
00158 class Command_SetPose(QUndoCommand):
00159
00160 def __init__(self, editor, element, position, orientation):
00161 QUndoCommand.__init__(self, "Position")
00162 self.editor = editor
00163
00164 self.element = element
00165
00166 self.time = time.time()
00167
00168 self.new_position = position
00169 self.new_orientation = orientation
00170 self.old_position = element.position
00171 self.old_orientation = element.orientation
00172
00173 def redo(self):
00174 self.element.position = self.new_position
00175 self.element.orientation = self.new_orientation
00176 self.editor.add_undo_level(4, [self.element])
00177
00178 def undo(self):
00179 self.element.position = self.old_position
00180 self.element.orientation = self.old_orientation
00181 self.editor.add_undo_level(4, [self.element])
00182
00183 def id(self):
00184 return 1
00185
00186 def mergeWith(self, command):
00187 if self.id() != command.id():
00188 return False
00189 if self.element is not command.element:
00190 return False
00191 if time.time() - self.time > 1.0:
00192 return False
00193
00194
00195 self.time = time.time()
00196 self.new_position = command.new_position
00197 self.new_orientation = command.new_orientation
00198 return True
00199
00200
00201 class Command_SetPosition(QUndoCommand):
00202
00203 def __init__(self, editor, element, position):
00204 QUndoCommand.__init__(self, "Position")
00205 self.editor = editor
00206
00207 self.element = element
00208
00209 self.new_position = position
00210 self.old_position = element.position
00211
00212 def redo(self):
00213 self.element.position = self.new_position
00214 self.editor.add_undo_level(4, [self.element])
00215
00216 def undo(self):
00217 self.element.position = self.old_position
00218 self.editor.add_undo_level(4, [self.element])
00219
00220
00221 class Command_SetOrientation(QUndoCommand):
00222
00223 def __init__(self, editor, element, orientation):
00224 QUndoCommand.__init__(self, "Orientation")
00225 self.editor = editor
00226
00227 self.element = element
00228
00229 self.new_orientation = orientation
00230 self.old_orientation = element.orientation
00231
00232 def redo(self):
00233 self.element.orientation = self.new_orientation
00234 self.editor.add_undo_level(4, [self.element])
00235
00236 def undo(self):
00237 self.element.orientation = self.old_orientation
00238 self.editor.add_undo_level(4, [self.element])
00239
00240
00241 class Command_SetValue(QUndoCommand):
00242
00243 def __init__(self, editor, element, symbol, value):
00244 QUndoCommand.__init__(self, "Value")
00245 self.editor = editor
00246
00247 self.element = element
00248 self.symbol = symbol
00249
00250 self.new_value = value
00251 self.old_value = element.value(symbol)
00252
00253 def redo(self):
00254 self.element.set_value(self.symbol, self.new_value)
00255 self.editor.add_undo_level(4, [self.element])
00256
00257 def undo(self):
00258 self.element.set_value(self.symbol, self.old_value)
00259 self.editor.add_undo_level(4, [self.element])
00260
00261
00262 class Command_SetParent(QUndoCommand):
00263
00264 def __init__(self, editor, element, parent_name, keep_absolute=True):
00265 QUndoCommand.__init__(self, "Parent")
00266 self.editor = editor
00267
00268 self.element = element
00269 self.keep_absolute = keep_absolute
00270
00271 self.new_parent_name = parent_name
00272 self.old_parent_name = element.parent
00273
00274 if self.keep_absolute:
00275 position, orientation = FromTransformStamped(
00276 element.tf_buffer.lookup_transform(
00277 parent_name, element.name, rospy.Time(0)))
00278 self.new_position = position
00279 self.new_orientation = orientation
00280
00281 self.old_position = element.position
00282 self.old_orientation = element.orientation
00283
00284 def redo(self):
00285 self.element.parent = self.new_parent_name
00286
00287 if self.keep_absolute:
00288 self.element.position = self.new_position
00289 self.element.orientation = self.new_orientation
00290
00291 self.editor.add_undo_level(4, [self.element])
00292
00293 def undo(self):
00294 self.element.parent = self.old_parent_name
00295
00296 if self.keep_absolute:
00297 self.element.position = self.old_position
00298 self.element.orientation = self.old_orientation
00299
00300 self.editor.add_undo_level(4, [self.element])
00301
00302 class Command_SetStyle(QUndoCommand):
00303
00304 def __init__(self, editor, element, style):
00305 QUndoCommand.__init__(self, "Style")
00306 self.editor = editor
00307
00308 self.old_element = element
00309
00310 if style == "plane":
00311 self.new_element = Object_Plane(element.name, element.position, element.orientation, element.parent)
00312 elif style == "cube":
00313 self.new_element = Object_Cube(element.name, element.position, element.orientation, element.parent)
00314 elif style == "sphere":
00315 self.new_element = Object_Sphere(element.name, element.position, element.orientation, element.parent)
00316 elif style == "axis":
00317 self.new_element = Object_Axis(element.name, element.position, element.orientation, element.parent)
00318 elif style == "mesh":
00319 self.new_element = Object_Mesh(element.name, element.position, element.orientation, element.parent)
00320 else:
00321 self.new_element = Frame(element.name, element.position, element.orientation, element.parent)
00322
00323 if editor.active_frame is element:
00324 self.was_active = True
00325 else:
00326 self.was_active = False
00327
00328 def redo(self):
00329 del self.editor.frames[self.old_element.name]
00330 self.old_element.hidden = True
00331
00332 self.editor.frames[self.new_element.name] = self.new_element
00333 self.new_element.hidden = False
00334
00335 self.editor.add_undo_level(1+4, [self.new_element, self.old_element])
00336
00337 if self.was_active:
00338 self.editor.active_frame = self.new_element
00339 self.editor.add_undo_level(2)
00340
00341 def undo(self):
00342 del self.editor.frames[self.new_element.name]
00343 self.new_element.hidden = True
00344
00345 self.editor.frames[self.old_element.name] = self.old_element
00346 self.old_element.hidden = False
00347
00348 self.editor.add_undo_level(1+4, [self.new_element, self.old_element])
00349
00350 if self.was_active:
00351 self.editor.active_frame = self.old_element
00352 self.editor.add_undo_level(2)
00353
00354 class Command_SetStyleColor(QUndoCommand):
00355
00356 def __init__(self, editor, element, color_rgba):
00357 QUndoCommand.__init__(self, "Style Color")
00358 self.editor = editor
00359
00360 self.element = element
00361 self.old_color = element.color
00362 self.new_color = color_rgba
00363
00364 def redo(self):
00365 self.element.set_color(self.new_color)
00366 self.editor.add_undo_level(4, [self.element])
00367
00368 def undo(self):
00369 self.element.set_color(self.old_color)
00370 self.editor.add_undo_level(4, [self.element])
00371
00372
00373 class Command_SetGeometry(QUndoCommand):
00374
00375 def __init__(self, editor, element, parameter, value):
00376 QUndoCommand.__init__(self, "Style Geometry")
00377 self.editor = editor
00378
00379 self.element = element
00380 self.parameter = parameter
00381 self.old_value = getattr(element, parameter)
00382 self.new_value = value
00383
00384 def redo(self):
00385 setattr(self.element, self.parameter, self.new_value)
00386 self.element.update_marker()
00387 self.editor.add_undo_level(4, [self.element])
00388
00389
00390 def undo(self):
00391 setattr(self.element, self.parameter, self.old_value)
00392 self.element.update_marker()
00393 self.editor.add_undo_level(4, [self.element])
00394
00395