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 time
00005 import pr2_common_action_msgs.msg as ca
00006
00007 class TuckTool(tu.ToolBase):
00008
00009 def __init__(self, rcommander):
00010 tu.ToolBase.__init__(self, rcommander, 'tuck', 'Tuck')
00011
00012 def fill_property_box(self, pbox):
00013 formlayout = pbox.layout()
00014
00015
00016 self.tuck_left = QComboBox(pbox)
00017 self.tuck_left.addItem('True')
00018 self.tuck_left.addItem('False')
00019
00020 self.tuck_right = QComboBox(pbox)
00021 self.tuck_right.addItem('True')
00022 self.tuck_right.addItem('False')
00023
00024 formlayout.addRow("&Left Arm", self.tuck_left)
00025 formlayout.addRow("&Right Arm", self.tuck_right)
00026 pbox.update()
00027
00028 def _create_node(self, name=None):
00029 left = ('True' == str(self.tuck_left.currentText()))
00030 right = ('True' == str(self.tuck_right.currentText()))
00031 if name == None:
00032 nname = self.name + str(self.counter)
00033 else:
00034 nname = name
00035 return TuckState(nname, left, right)
00036
00037 def _node_selected(self, node):
00038 self.tuck_left.setCurrentIndex(self.tuck_left.findText(str(node.tuck_left)))
00039 self.tuck_right.setCurrentIndex(self.tuck_right.findText(str(node.tuck_right)))
00040
00041 def reset(self):
00042 self.tuck_left.setCurrentIndex(self.tuck_left.findText('True'))
00043 self.tuck_right.setCurrentIndex(self.tuck_right.findText('True'))
00044
00045 def get_smach_class(self):
00046 return TuckState
00047
00048
00049 class TuckState(tu.SimpleStateBase):
00050 def __init__(self, name, tuck_left, tuck_right):
00051 tu.SimpleStateBase.__init__(self, name, \
00052 'tuck_arms', ca.TuckArmsAction,
00053 goal_cb_str = 'ros_goal')
00054
00055 self.tuck_left = tuck_left
00056 self.tuck_right = tuck_right
00057
00058 def ros_goal(self, userdata, default_goal):
00059 goal = ca.TuckArmsGoal()
00060 goal.tuck_left = self.tuck_left
00061 goal.tuck_right = self.tuck_right
00062 print 'GOAL', goal.tuck_left, goal.tuck_right
00063 return goal
00064
00065 def __getstate__(self):
00066 state = tu.SimpleStateBase.__getstate__(self)
00067 my_state = [self.tuck_left, self.tuck_right]
00068 return {'simple_state': state, 'self': my_state}
00069
00070 def __setstate__(self, state):
00071 tu.SimpleStateBase.__setstate__(self, state['simple_state'])
00072 self.tuck_left, self.tuck_right = state['self']