00001 import tool_utils as tu
00002 from PyQt4.QtGui import *
00003 from PyQt4.QtCore import *
00004 import smach
00005 import rospy
00006 from tf_broadcast_server.srv import BroadcastTransform, GetTransforms, ClearTransforms
00007
00008 import tf_utils as tfu
00009 import tool_utils as tu
00010
00011 class BaseFrameBox(tu.FrameBox):
00012
00013 def __init__(self, frame_service):
00014 tu.FrameBox.__init__(self, frame_service)
00015 self.baseframes = ['/map', '/base_link']
00016
00017 def create_box(self, pbox):
00018 tu.ComboBox.create_box(self, pbox)
00019 possible_frames = self.frames_service().frames
00020 for f in self.baseframes:
00021 if f in possible_frames:
00022 self.combobox.addItem(f)
00023 self.setEnabled = self.combobox.setEnabled
00024 return self.combobox
00025
00026
00027
00028 class FreezeFrameTool(tu.ToolBase):
00029
00030 def __init__(self, rcommander):
00031 tu.ToolBase.__init__(self, rcommander, 'freeze_frame', 'Freeze Frame', FreezeFrameState)
00032 self.default_frame = '/l_gripper_tool_frame'
00033 self.default_baseframe = '/base_link'
00034 self.tf_listener = rcommander.tf_listener
00035 self.frames_service = rospy.ServiceProxy('get_transforms', GetTransforms)
00036 self.clear_frames_service = rospy.ServiceProxy('clear_all_transforms', ClearTransforms)
00037
00038 def fill_property_box(self, pbox):
00039 formlayout = pbox.layout()
00040 self.base_frame_box = BaseFrameBox(self.frames_service)
00041 self.frame_box = tu.FrameBox(self.frames_service)
00042
00043
00044
00045
00046 self.clear_frames_button = QPushButton(pbox)
00047 self.clear_frames_button.setText('Clear Frames')
00048 self.rcommander.connect(self.clear_frames_button, SIGNAL('clicked()'), self.clear_frames_button_cb)
00049
00050 formlayout.addRow("&Base Frame", self.base_frame_box.create_box(pbox))
00051 formlayout.addRow("&Frame", self.frame_box.create_box(pbox))
00052 formlayout.addRow(self.clear_frames_button)
00053
00054 self.reset()
00055 pbox.update()
00056
00057 def clear_frames_button_cb(self):
00058 self.clear_frames_service()
00059
00060 def new_node(self, name=None):
00061 if name == None:
00062 nname = self.name + str(self.counter)
00063 else:
00064 nname = name
00065 return FreezeFrameState(nname, self.base_frame_box.text(), self.frame_box.text())
00066
00067 def set_node_properties(self, node):
00068 self.base_frame_box.set_text(node.base_frame)
00069 self.frame_box.set_text(node.frame_to_clone)
00070
00071 def reset(self):
00072 self.base_frame_box.set_text(self.default_baseframe, create=False)
00073 self.frame_box.set_text(self.default_frame, create=False)
00074
00075
00076 class FreezeFrameState(tu.StateBase):
00077
00078 def __init__(self, name, frame_to_clone, base_frame):
00079 tu.StateBase.__init__(self, name)
00080 self.frame_to_clone = frame_to_clone
00081 self.base_frame = base_frame
00082
00083 def get_smach_state(self):
00084 return FreezeFrameStateSmach(self.get_name(), self.frame_to_clone, self.base_frame)
00085
00086
00087 class FreezeFrameStateSmach(smach.State):
00088
00089 def __init__(self, new_frame_name, frame_to_clone, base_frame):
00090 smach.State.__init__(self, outcomes = ['succeeded'], input_keys = [], output_keys = [])
00091 self.new_frame_name = new_frame_name
00092 self.base_frame = base_frame
00093 self.frame_to_clone = frame_to_clone
00094 self.broadcast_transform_srv = rospy.ServiceProxy('broadcast_transform', BroadcastTransform)
00095
00096 def set_robot(self, robot):
00097 self.robot = robot
00098
00099 def execute(self, userdata):
00100 base_T_clone = tfu.tf_as_matrix(self.robot.tf_listener.lookupTransform(self.base_frame,
00101 self.frame_to_clone, rospy.Time(0)))
00102 pose = tfu.stamp_pose(tfu.mat_to_pose(base_T_clone), self.base_frame)
00103 self.broadcast_transform_srv(self.new_frame_name, pose)
00104 return 'succeeded'
00105