frame_editor: interface_services.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import copy
00004 
00005 import rospy
00006 
00007 from frame_editor.objects import *
00008 from frame_editor.commands import *
00009 from frame_editor.interface import Interface
00010 
00011 from frame_editor.constructors_geometry import *
00012 from frame_editor.constructors_std import *
00013 
00014 from frame_editor.srv import *
00015 
00016 
00017 class FrameEditor_Services(Interface):
00018 
00019     def __init__(self, frame_editor):
00020 
00021         self.editor = frame_editor
00022 
00023         rospy.Service("align_frame", AlignFrame, self.callback_align_frame)
00024         rospy.Service("edit_frame", EditFrame, self.callback_edit_frame)
00025         rospy.Service("get_frame", GetFrame, self.callback_get_frame)
00026         rospy.Service("remove_frame", RemoveFrame, self.callback_remove_frame)
00027         rospy.Service("set_frame", SetFrame, self.callback_set_frame)
00028         rospy.Service("set_parent", SetParentFrame, self.callback_set_parent_frame)
00029         rospy.Service("copy_frame", CopyFrame, self.callback_copy_frame)
00030 
00031 
00032     def callback_align_frame(self, request):
00033         print "> Request to align frame", request.name, "with frame", request.source_name, "mode", request.mode
00034 
00035         response = AlignFrameResponse()
00036         response.error_code = 0
00037 
00038         if request.name == "":
00039             print " Error: No name given"
00040             response.error_code = 1
00041 
00042         elif request.source_name == "":
00043             print " Error: No source name given"
00044             response.error_code = 3
00045 
00046         elif request.name not in self.editor.frames:
00047             print " Error: Frame not found:", request.name
00048             response.error_code = 2
00049 
00050         else:
00051             frame = self.editor.frames[request.name]
00052 
00053             m = request.mode
00054             mode = []
00055             if m & 1: mode.append("x")
00056             if m & 2: mode.append("y")
00057             if m & 4: mode.append("z")
00058             if m & 8: mode.append("a")
00059             if m & 16: mode.append("b")
00060             if m & 32: mode.append("c")
00061 
00062             self.editor.command(Command_AlignElement(self.editor, frame, request.source_name, mode))
00063 
00064         return response
00065 
00066 
00067     def callback_edit_frame(self, request):
00068         print "> Request to edit frame", request.name
00069 
00070         response = EditFrameResponse()
00071         response.error_code = 0
00072 
00073         if request.name == "":
00074             ## Reset
00075             self.editor.command(Command_SelectElement(self.editor, None))
00076 
00077         elif request.name not in self.editor.frames:
00078             print " Error: Frame not found:", request.name
00079             response.error_code = 2
00080 
00081         else:
00082             ## Set
00083             self.editor.command(Command_SelectElement(self.editor, self.editor.frames[request.name]))
00084 
00085         return response
00086 
00087 
00088     def callback_get_frame(self, request):
00089         print "> Request to get frame", request.name
00090 
00091         response = GetFrameResponse()
00092         response.error_code = 0
00093 
00094         if request.name == "":
00095             print " Error: No name given"
00096             response.error_code = 1
00097 
00098         elif request.name not in self.editor.frames:
00099             print " Error: Frame not found:", request.name
00100             response.error_code = 2
00101 
00102         else:
00103             f = self.editor.frames[request.name]
00104             f.print_all()
00105             response.name = f.name
00106             response.parent = f.parent
00107             response.pose = ToPose(f.position, f.orientation)
00108 
00109         return response
00110 
00111 
00112     def callback_remove_frame(self, request):
00113         print "> Request to remove frame", request.name
00114 
00115         response = RemoveFrameResponse()
00116         response.error_code = 0
00117 
00118         if request.name == "":
00119             print " Error: No name given"
00120             response.error_code = 1
00121 
00122         elif request.name not in self.editor.frames:
00123             print " Error: Frame not found:", request.name
00124             response.error_code = 2
00125 
00126         else:
00127             self.editor.command(Command_RemoveElement(self.editor, self.editor.frames[request.name]))
00128 
00129         return response
00130 
00131 
00132     def callback_set_frame(self, request):
00133         print "> Request to set (or add) frame", request.name, request.parent
00134 
00135         response = SetFrameResponse()
00136         response.error_code = 0
00137 
00138         if request.name == "":
00139             print " Error: No name given"
00140             response.error_code = 1
00141 
00142         else:
00143             if request.parent == "":
00144                 request.parent = "world"
00145 
00146             f = Frame(request.name,
00147                       FromPoint(request.pose.position),
00148                       FromQuaternion(request.pose.orientation),
00149                       request.parent)
00150             self.editor.command(Command_AddElement(self.editor, f))
00151 
00152         return response
00153 
00154 
00155     def callback_set_parent_frame(self, request):
00156         print "> Request to set parent_frame", request.name, request.parent
00157 
00158         response = SetParentFrameResponse()
00159         response.error_code = 0
00160 
00161         if request.name == "":
00162             print " Error: No frame_name given"
00163             response.error_code = 1
00164 
00165         elif request.parent == "":
00166             print " Error: No parent_name given"
00167             response.error_code = 2
00168 
00169         else:
00170             f = self.editor.frames[request.name]
00171             self.editor.command(Command_SetParent(self.editor, f, request.parent, request.keep_absolute))
00172 
00173         return response
00174 
00175 
00176     def callback_copy_frame(self, request):
00177         print "> Request to copy frame '" + request.source_name + "' with new name '" + request.name + "' and new parent name '" + request.parent + "'"
00178 
00179         response = CopyFrameResponse()
00180         response.error_code = 0
00181 
00182         if request.name == "":
00183             print " Error: No name given"
00184             response.error_code = 1
00185 
00186         elif request.source_name == "":
00187             print " Error: No source name given"
00188             response.error_code = 3
00189 
00190         else:
00191             ## If not existing yet: create frame
00192             if request.name not in self.editor.frames:
00193                 if request.source_name in self.editor.frames:
00194                     frame = copy.deepcopy(self.editor.frames[request.source_name])
00195                     frame.name = request.name
00196                 else:
00197                     frame = Frame(request.name, parent=request.source_name)
00198                 self.editor.command(Command_AddElement(self.editor, frame))
00199             else:
00200                 ## Align with source frame
00201                 frame = self.editor.frames[request.name]
00202                 frame_tf = frame.tf_buffer.can_transform(request.name, request.source_name, rospy.Time.now(), rospy.Duration(1.0), rospy.Duration(0.01))
00203                 if not frame_tf:
00204                     print " Error: tf can not transform. (Align)"
00205                     response.error_code = 4
00206                 self.editor.command(Command_AlignElement(self.editor, frame, request.source_name, ['x', 'y', 'z', 'a', 'b', 'c']))
00207 
00208             ## Set parent
00209             if (request.parent != "") and (frame.parent != request.parent):
00210                 ## Make sure the listener knows the new frame / its new aligned position
00211                 self.editor.broadcast() # apparently you can't broadcast often enough
00212                 frame_tf = frame.tf_buffer.can_transform(frame.parent, frame.name, rospy.Time.now(), rospy.Duration(1.0), rospy.Duration(0.01))
00213                 new_tf = frame.tf_buffer.can_transform(request.parent, frame.name, rospy.Time.now(), rospy.Duration(1.0), rospy.Duration(0.01))
00214                 if not (frame_tf and new_tf):
00215                     print " Error: tf can not transform."
00216                     response.error_code = 4
00217                 self.editor.command(Command_SetParent(self.editor, frame, request.parent, True))
00218 
00219         return response
00220 
00221 # eof