Go to the documentation of this file.00001 import tool_utils as tu
00002 from PyQt4.QtGui import *
00003 from PyQt4.QtCore import *
00004 import numpy as np
00005
00006
00007 class Point3DTool(tu.ToolBase):
00008
00009 def __init__(self, rcommander):
00010 tu.ToolBase.__init__(self, rcommander, 'point3d', 'Point 3D', Point3DState)
00011 self.default_frame = 'base_link'
00012
00013 def fill_property_box(self, pbox):
00014 formlayout = pbox.layout()
00015
00016 self.xline = QLineEdit(pbox)
00017 self.yline = QLineEdit(pbox)
00018 self.zline = QLineEdit(pbox)
00019 self.frameline = QLineEdit(pbox)
00020 self.pose_button = QPushButton(pbox)
00021 self.pose_button.setText('Get Pose')
00022 self.pose_button.setEnabled(False)
00023
00024 formlayout.addRow("&x", self.xline)
00025 formlayout.addRow("&y", self.yline)
00026 formlayout.addRow("&z", self.zline)
00027 formlayout.addRow('&frame', self.frameline)
00028 formlayout.addRow(self.pose_button)
00029 self.reset()
00030 pbox.update()
00031
00032 def new_node(self, name=None):
00033 point = [float(str(self.xline.text())), float(str(self.yline.text())), float(str(self.zline.text()))]
00034 frame = str(self.frameline.text())
00035 if name == None:
00036 nname = self.name + str(self.counter)
00037 else:
00038 nname = name
00039 return Point3DState(nname, point, frame)
00040
00041 def set_node_properties(self, node):
00042 self.xline.setText(str(node.point[0]))
00043 self.yline.setText(str(node.point[1]))
00044 self.zline.setText(str(node.point[2]))
00045 self.frameline.setText(node.frame)
00046
00047 def reset(self):
00048 self.xline.setText(str(0.))
00049 self.yline.setText(str(0.))
00050 self.zline.setText(str(0.))
00051 self.frameline.setText(self.default_frame)
00052
00053
00054 class Point3DState(tu.InfoStateBase):
00055
00056 def __init__(self, name, point, frame):
00057 tu.InfoStateBase.__init__(self, name)
00058 self.point = point
00059 self.frame = frame
00060
00061 def set_info(self, info):
00062 self.point, self.frame = info
00063
00064 def get_info(self):
00065 return [self.point, self.frame]
00066
00067 def __getstate__(self):
00068 state = tu.InfoStateBase.__getstate__(self)
00069 my_state = [self.point, self.frame]
00070 return {'simple_state': state, 'self': my_state}
00071
00072 def __setstate__(self, state):
00073 tu.InfoStateBase.__setstate__(self, state['simple_state'])
00074 self.point, self.frame = state['self']