editor.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import os
4 import sys
5 
6 import time
7 import threading
8 import yaml
9 
10 import rospy
11 import rosparam
12 import rospkg
13 
14 from frame_editor.objects import *
15 from frame_editor.commands import *
16 
19 
20 from python_qt_binding import QtCore
21 from python_qt_binding.QtWidgets import QUndoStack
22 
23 ## Views
24 from frame_editor.interface_interactive_marker import FrameEditor_InteractiveMarker
25 from frame_editor.interface_services import FrameEditor_Services
26 from frame_editor.interface_markers import FrameEditor_Markers
27 from frame_editor.interface_tf import FrameEditor_TF
28 
29 
30 class FrameEditor(QtCore.QObject):
31 
32  def __init__(self):
33  Frame.init_tf()
34  super(FrameEditor, self).__init__()
35 
36  self.frames = {}
37  self.active_frame = None
38 
39  ## Undo/Redo
40  self.observers = []
41  self.undo_level = 0
42  self.undo_elements = []
43  self.undo_stack = QUndoStack()
44  self.undo_stack.indexChanged.connect(self.undo_stack_changed)
45  self.__command_lock = threading.Lock()
46 
47  self.namespace = "frame_editor"
48  self.full_file_path = None
49 
50 
51  def get_file_name(self):
52  if self.full_file_path is None:
53  return ""
54  else:
55  return os.path.basename(self.full_file_path)
56 
57  def get_full_file_path(self):
58  if self.full_file_path is None:
59  return ""
60  else:
61  return self.full_file_path
62 
63  ## Undo/Redo ##
64 
65  @QtCore.Slot(int)
66  def undo_stack_changed(self, idx):
67  '''Updates all observers, whenever a command has been undone/redone'''
68  self.update_obsevers(self.undo_level)
69 
70  def add_undo_level(self, level, elements=None):
71  '''Used by commands to add a level for updating'''
72  self.undo_level = self.undo_level | level
73  if elements:
74  self.undo_elements.extend(elements)
75 
76  def command(self, command):
77  '''Push a command to the stack (blocking)'''
78  with self.__command_lock:
79  self.undo_stack.push(command)
80 
81 
82  def update_obsevers(self, level):
83  '''Updates all registered observers and resets the undo_level'''
84  for observer in self.observers:
85  observer.update(self, level, self.undo_elements)
86  self.undo_level = 0
87  self.undo_elements = []
88 
89  def broadcast(self):
90  for observer in self.observers:
91  observer.broadcast(self)
92 
93  @staticmethod
94  def tf_dict():
95  y = Frame.tf_buffer.all_frames_as_yaml()
96  d = yaml.load(y)
97  if isinstance(d, dict):
98  return d
99  else:
100  rospy.logwarn('Got invalid yaml from tf2: '+y)
101  return {}
102 
103  @staticmethod
104  def frame_is_temporary(frame_id):
105  return frame_id.startswith('_')
106 
107  @staticmethod
108  def all_frame_ids(include_temp=True):
109  return [f for f in FrameEditor.tf_dict() if
110  not FrameEditor.frame_is_temporary(f) or include_temp]
111 
112  def iter_frames(self, include_temp=True):
113  for f in self.frames.itervalues():
114  if not self.frame_is_temporary(f.name) or include_temp:
115  yield f
116 
117 
118  ## PRINT ##
119 
120  def print_all(self):
121  print("> Printing all frames")
122 
123  for frame in self.frames:
124  frame.print_all()
125 
126 
127  ## FILE I/O ##
128 
129  def load_file(self, file_name):
130  if file_name:
131  print("> Loading file")
132  data = rosparam.load_file(file_name, self.namespace)[0][0]
133  self.load_data(data)
134  else:
135  ## Clear everything
136  self.command(Command_ClearAll(self))
137 
138  self.undo_stack.clear()
139 
140  self.full_file_path = file_name
141  return True
142 
143  def load_params(self, namespace):
144  if not rosparam.list_params(namespace):
145  print("> No data to load")
146  else:
147  data = rosparam.get_param(namespace)
148  self.load_data(data)
149 
150  def load_data(self, data):
151 
152  self.undo_stack.beginMacro("Import file")
153 
154  ## Import data
155  for name, frame in data["frames"].items():
156  t = frame["position"]
157  o = frame["orientation"]
158 
159  if "style" in frame:
160  style = frame["style"]
161  else:
162  style = "none"
163 
164  if "data" in frame:
165  dat = frame["data"]
166  if "color" in dat:
167  color = dat["color"]
168  else:
169  color = (0.0, 0.5, 0.5, 0.75)
170  if "package" not in dat:
171  dat["package"] = ""
172 
173  position = (t["x"], t["y"], t["z"])
174  orientation = (o["x"], o["y"], o["z"], o["w"])
175 
176  if style == "plane":
177  f = Object_Plane(name, position, orientation, frame["parent"], dat["length"], dat["width"])
178  f.set_color(color)
179  elif style == "cube":
180  f = Object_Cube(name, position, orientation, frame["parent"], dat["length"], dat["width"], dat["height"])
181  f.set_color(color)
182  elif style == "sphere":
183  f = Object_Sphere(name, position, orientation, frame["parent"], dat["diameter"])
184  f.set_color(color)
185  elif style == "axis":
186  f = Object_Axis(name, position, orientation, frame["parent"], dat["length"], dat["width"])
187  f.set_color(color)
188  elif style == "mesh":
189  f = Object_Mesh(name, position, orientation, frame["parent"], dat["package"], dat["path"], dat["scale"])
190  f.set_color(color)
191  else:
192  f = Frame(name, position, orientation, frame["parent"])
193 
194  self.command(Command_AddElement(self, f))
195 
196  self.undo_stack.endMacro()
197 
198  print("> Loading done")
199 
200  def save_file(self, filename):
201 
202  ## Data
203  data = {}
204  frames = {}
205 
206  for frame in self.iter_frames(include_temp=False):
207  t = {}
208  t["x"] = float(frame.position[0])
209  t["y"] = float(frame.position[1])
210  t["z"] = float(frame.position[2])
211 
212  o = {}
213  o["x"] = float(frame.orientation[0])
214  o["y"] = float(frame.orientation[1])
215  o["z"] = float(frame.orientation[2])
216  o["w"] = float(frame.orientation[3])
217 
218  f = {}
219  f["parent"] = frame.parent
220  f["position"] = t
221  f["orientation"] = o
222 
223  f["style"] = frame.style
224 
225  if frame.style == "plane":
226  f["data"] = { "length": frame.length, "width":frame.width, "color": frame.color }
227 
228  elif frame.style == "cube":
229  f["data"] = { "length": frame.length, "width": frame.width, "height": frame.height , "color": frame.color}
230 
231  elif frame.style == "sphere":
232  f["data"] = { "diameter": frame.diameter, "color": frame.color }
233 
234  elif frame.style == "axis":
235  f["data"] = { "length": frame.length, "width": frame.width, "color": frame.color }
236 
237  elif frame.style == "mesh":
238  self.update_file_format(frame)
239  f["data"] = { "package" : frame.package, "path" : frame.path, "scale" : frame.scale, "color": frame.color }
240 
241  frames[frame.name] = f
242 
243  data["frames"] = frames
244 
245  ## To parameter server
246  rospy.set_param(self.namespace, data)
247  print(rospy.get_param(self.namespace))
248 
249  ## Dump param to file
250  if filename == '':
251  filename = self.full_file_path
252  print("Saving to file {}".format(filename))
253  rosparam.dump_params(filename, self.namespace)
254  print("Saving done")
255 
256  self.full_file_path = filename
257  return True
258 
259  def update_file_format(self, frame):
260  if frame.package == "" and frame.path != "":
261  try:
262  import rospkg
263  import os
264  from python_qt_binding import QtWidgets
265  rospackage = rospkg.get_package_name(frame.path)
266  if rospackage is not None:
267  rel_path = os.path.relpath(frame.path , rospkg.RosPack().get_path(rospackage))
268  reply = QtWidgets.QMessageBox.question(None, "Convert absolute path to rospack+relative path?",
269  "The absolute path to your selected mesh can be converted to rospack+relative path."+
270  "This gives you more reliabilaty to reuse your saved configuration"+
271  "if your meshes are stored in rospackages\n\n"+
272  "Do you want to convert your configuration?\n"+
273  "Convert:\n'{}'\nto:\n'{}' and\n '{}'\n".format(frame.path, rospackage, rel_path),
274  QtWidgets.QMessageBox.Yes |
275  QtWidgets.QMessageBox.No,
276  QtWidgets.QMessageBox.Yes)
277 
278  if reply == QtWidgets.QMessageBox.Yes:
279  print("Saving: package: {} + relative path: {}".format(rospackage, rel_path))
280  frame.package = rospackage
281  frame.path = rel_path
282  return
283  except:
284  # Do nothing if conversion fails
285  pass
286  else:
287  # Do nothing if conversion not needed
288  pass
289 
290  def run(self):
291  print("> Going for some spins")
292  rate = rospy.Rate(200) # hz
293  while not rospy.is_shutdown():
294  self.broadcast()
295  rate.sleep()
296 
297  def parse_args(self, argv):
298  ## Args ##
299 
301  from argparse import ArgumentParser
302  parser = ArgumentParser()
303  # Add argument(s) to the parser.
304  #parser.add_argument("-q", "--quiet", action="store_true",
305  # dest="quiet",
306  # help="Put plugin in silent mode")
307  parser.add_argument("-l", "--load", action="append",
308  dest="file",
309  help="Load a file at startup. [rospack filepath/file]")
310 
311  args, unknowns = parser.parse_known_args(argv)
312  print('arguments: {}'.format(args))
313  if unknowns:
314  print('unknown parameters found: {}'.format(unknowns))
315 
316  ## Load file ##
317 
318  if args.file:
319  arg_path = args.file[0].split()
320  if len(arg_path) == 1:
321  #load file
322  filename = arg_path[0]
323  print("Loading {}".format(filename))
324  success = self.load_file(str(filename))
325  elif len(arg_path) == 2:
326  #load rospack
327  rospack = rospkg.RosPack()
328  filename = os.path.join(rospack.get_path(arg_path[0]), arg_path[1])
329  print("Loading {}".format(filename))
330  success = self.load_file(str(filename))
331  else:
332  print("Load argument not understood! --load {}".format(arg_path))
333  print("Please use --load 'myRosPackage pathInMyPackage/myYaml.yaml'")
334  print("or use --load 'fullPathToMyYaml.yaml'")
335  success = None
336 
337  if success:
338  return filename
339  elif success == False:
340  print("ERROR LOADING FILE")
341  return ''
342 
343  def init_views(self):
344  ## Views
349 
350 if __name__ == "__main__":
351 
352  rospy.init_node('frame_editor')
353 
354  editor = FrameEditor()
355  # editor.load_params(rospy.get_name())
356 
357  editor.parse_args(sys.argv[1:])
358  editor.init_views()
359 
360  print("Frame editor ready!")
361  editor.run()
362 
363 # eof
def load_params(self, namespace)
Definition: editor.py:143
def load_file(self, file_name)
FILE I/O ##.
Definition: editor.py:129
def command(self, command)
Definition: editor.py:76
def frame_is_temporary(frame_id)
Definition: editor.py:104
def parse_args(self, argv)
Process standalone plugin command-line arguments.
Definition: editor.py:297
def undo_stack_changed(self, idx)
Undo/Redo ##.
Definition: editor.py:66
def add_undo_level(self, level, elements=None)
Definition: editor.py:70
def update_file_format(self, frame)
Definition: editor.py:259
def load_data(self, data)
Definition: editor.py:150
def save_file(self, filename)
Definition: editor.py:200
def iter_frames(self, include_temp=True)
Definition: editor.py:112
full_file_path
Clear everything.
Definition: editor.py:48
def print_all(self)
PRINT ##.
Definition: editor.py:120
def all_frame_ids(include_temp=True)
Definition: editor.py:108
def update_obsevers(self, level)
Definition: editor.py:82


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