interface_services.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 os
8 
9 from frame_editor.objects import *
10 from frame_editor.commands import *
11 from frame_editor.interface import Interface
12 
15 
16 from frame_editor.srv import *
17 
18 
20 
21  def __init__(self, frame_editor):
22 
23  self.editor = frame_editor
24 
25  rospy.Service("~align_frame", AlignFrame, self.callback_align_frame)
26  rospy.Service("~edit_frame", EditFrame, self.callback_edit_frame)
27  rospy.Service("~get_frame", GetFrame, self.callback_get_frame)
28  rospy.Service("~remove_frame", RemoveFrame, self.callback_remove_frame)
29  rospy.Service("~set_frame", SetFrame, self.callback_set_frame)
30  rospy.Service("~set_parent", SetParentFrame, self.callback_set_parent_frame)
31  rospy.Service("~copy_frame", CopyFrame, self.callback_copy_frame)
32 
33  rospy.Service("~load_yaml", LoadYaml, self.callback_load_yaml)
34  rospy.Service("~save_yaml", SaveYaml, self.callback_save_yaml)
35 
36 
37  def callback_align_frame(self, request):
38  print("> Request to align frame {} with frame {} mode {}".format(request.name, request.source_name, request.mode))
39 
40  response = AlignFrameResponse()
41  response.error_code = 0
42 
43  if request.name == "":
44  print(" Error: No name given")
45  response.error_code = 1
46 
47  elif request.source_name == "":
48  print(" Error: No source name given")
49  response.error_code = 3
50 
51  elif request.name not in self.editor.frames:
52  print(" Error: Frame not found: {}".format(request.name))
53  response.error_code = 2
54 
55  else:
56  frame = self.editor.frames[request.name]
57 
58  m = request.mode
59  mode = []
60  if m & 1: mode.append("x")
61  if m & 2: mode.append("y")
62  if m & 4: mode.append("z")
63  if m & 8: mode.append("a")
64  if m & 16: mode.append("b")
65  if m & 32: mode.append("c")
66 
67  self.editor.command(Command_AlignElement(self.editor, frame, request.source_name, mode))
68 
69  return response
70 
71 
72  def callback_edit_frame(self, request):
73  print("> Request to edit frame {}".format(request.name))
74 
75  response = EditFrameResponse()
76  response.error_code = 0
77 
78  if request.name == "":
79  ## Reset
80  self.editor.command(Command_SelectElement(self.editor, None))
81 
82  elif request.name not in self.editor.frames:
83  print(" Error: Frame not found: {}".format(request.name))
84  response.error_code = 2
85 
86  else:
87  ## Set
88  self.editor.command(Command_SelectElement(self.editor, self.editor.frames[request.name]))
89 
90  return response
91 
92 
93  def callback_get_frame(self, request):
94  print("> Request to get frame {}".format(request.name))
95 
96  response = GetFrameResponse()
97  response.error_code = 0
98 
99  if request.name == "":
100  print(" Error: No name given")
101  response.error_code = 1
102 
103  elif request.name not in self.editor.frames:
104  print(" Error: Frame not found: {}".format(request.name))
105  response.error_code = 2
106 
107  else:
108  f = self.editor.frames[request.name]
109  f.print_all()
110  response.name = f.name
111  response.parent = f.parent
112  response.pose = ToPose(f.position, f.orientation)
113 
114  return response
115 
116 
117  def callback_remove_frame(self, request):
118  print("> Request to remove frame {}".format(request.name))
119 
120  response = RemoveFrameResponse()
121  response.error_code = 0
122 
123  if request.name == "":
124  print(" Error: No name given")
125  response.error_code = 1
126 
127  elif request.name not in self.editor.frames:
128  print(" Error: Frame not found: {}".format(request.name))
129  response.error_code = 2
130 
131  else:
132  self.editor.command(Command_RemoveElement(self.editor, self.editor.frames[request.name]))
133 
134  return response
135 
136 
137  def callback_set_frame(self, request):
138  print("> Request to set (or add) frame {} {}".format(request.name, request.parent))
139 
140  response = SetFrameResponse()
141 
142  if request.name == "":
143  print(" Error: No name given")
144  response.error_code = 1
145  return response
146 
147  if request.parent == "":
148  if request.name in self.editor.frames:
149  request.parent = self.editor.frames[request.name].parent
150  else:
151  print("Error: No parent given and frame previously not existing")
152  response.error_code = 2
153  return response
154 
155  f = Frame(request.name,
156  FromPoint(request.pose.position),
157  FromQuaternion(request.pose.orientation),
158  request.parent)
159  self.editor.command(Command_AddElement(self.editor, f))
160 
161  response.error_code = 0
162  return response
163 
164 
165  def callback_set_parent_frame(self, request):
166  print("> Request to set parent_frame {} {}".format(request.name, request.parent))
167 
168  response = SetParentFrameResponse()
169  response.error_code = 0
170 
171  if request.name == "":
172  print(" Error: No frame_name given")
173  response.error_code = 1
174 
175  elif request.parent == "":
176  print(" Error: No parent_name given")
177  response.error_code = 2
178 
179  else:
180  f = self.editor.frames[request.name]
181  self.editor.command(Command_SetParent(self.editor, f, request.parent, request.keep_absolute))
182 
183  return response
184 
185  def callback_load_yaml(self, request):
186  print("> Request to load yaml file:'{}'".format(request.filename))
187 
188  response = LoadYamlResponse()
189  try:
190  self.editor.load_file(os.path.expanduser(request.filename))
191  response.success = True
192  response.message = "file loaded"
193  except Exception as e:
194  response.success = False
195  response.message = "Exception: {}".format(str(e))
196 
197  return response
198 
199  def callback_save_yaml(self, request):
200  print("> Request to save yaml file to:'{}'".format(request.filename))
201 
202  response = SaveYamlResponse()
203  try:
204  self.editor.save_file(os.path.expanduser(request.filename))
205  response.success = True
206  response.message = "file saved"
207  except Exception as e:
208  response.success = False
209  response.message = "Exception: {}".format(str(e))
210 
211  return response
212 
213  def callback_copy_frame(self, request):
214  print("> Request to copy frame '{}' with new name '{}' and new parent name '{}'".format(request.source_name, request.name, request.parent))
215 
216  response = CopyFrameResponse()
217  response.error_code = 0
218 
219  if request.name == "":
220  print(" Error: No name given")
221  response.error_code = 1
222 
223  elif request.source_name == "":
224  print(" Error: No source name given")
225  response.error_code = 3
226 
227  else:
228  t = time.time()
229 
230  try:
231  # If not existing yet: create frame
232  if request.name not in self.editor.frames:
233  print(">> add")
234 
235  # No parent specified: use source's parent
236  if request.parent == "":
237  if request.source_name in self.editor.frames:
238  request.parent = self.editor.frames[request.source_name].parent
239  else:
240  print(" Error: No parent name given")
241  response.error_code = 3
242  return response
243 
244  Frame.wait_for_transform(request.source_name, request.parent, rospy.Duration(1.0))
245  self.editor.command(Command_CopyElement(self.editor, request.name, request.source_name, request.parent))
246  Frame.wait_for_transform(request.parent, request.name, rospy.Duration(1.0))
247 
248 
249  else:
250  frame = self.editor.frames[request.name]
251 
252  Frame.wait_for_transform(request.source_name, request.parent, rospy.Duration(1.0))
253  if (request.parent != "") and (frame.parent != request.parent):
254  print(">> rebase")
255  self.editor.command(Command_RebaseElement(self.editor, frame, request.source_name, request.parent))
256  else:
257  print(">> align")
258  self.editor.command(Command_AlignElement(self.editor, frame, request.source_name, ['x', 'y', 'z', 'a', 'b', 'c']))
259  Frame.wait_for_transform(frame.parent, frame.name, rospy.Duration(1.0))
260 
261  except Exception as e:
262  print("Error: unhandled exception {}".format(e))
263  response.error_code = 9
264 
265  print(time.time() - t)
266 
267  return response
268 
269 # eof


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