rcommander: linear_move_tool.py Source File

Go to the documentation of this file.
00001 import tool_utils as tu
00002 #import smach_ros
00003 from PyQt4.QtGui import *
00004 from PyQt4.QtCore import *
00005 import rospy
00006 import tf_utils as tfu
00007 import tf.transformations as tr
00008 import numpy as np
00009 #import move_base_msgs.msg as mm
00010 #import math
00011 from object_manipulator.convert_functions import *
00012 import ptp_arm_action.msg as ptp
00013 import math
00014 import point_tool as ptl
00015 
00016 ##
00017 # Options: stopping criteria
00018 #          relative or absolute
00019 ##
00020 
00021 #
00022 # controller and view
00023 # create and edits smach states
00024 class LinearMoveTool(tu.ToolBase):
00025 
00026     LEFT_TIP = 'l_gripper_tool_frame'
00027     RIGHT_TIP = 'r_gripper_tool_frame'
00028 
00029     def __init__(self, rcommander):
00030         tu.ToolBase.__init__(self, rcommander, 'linear_move', 'Linear Move')
00031         self.default_frame = 'base_link'
00032         self.tf_listener = rcommander.tf_listener
00033 
00034     def fill_property_box(self, pbox):
00035         formlayout = pbox.layout()
00036 
00037         self.arm_box = QComboBox(pbox)
00038         self.arm_box.addItem('left')
00039         self.arm_box.addItem('right')
00040 
00041         self.motion_box = QComboBox(pbox)
00042         self.motion_box.addItem('relative')
00043         self.motion_box.addItem('absolute')
00044 
00045         self.source_box = QComboBox(pbox)
00046         self.source_box.addItem(' ')
00047         nodes = self.rcommander.global_nodes(ptl.Point3DState)
00048         for n in nodes:
00049             self.source_box.addItem(n)
00050         self.rcommander.connect(self.source_box, SIGNAL('currentIndexChanged(int)'), self.source_changed)
00051 
00052         self.xline = QLineEdit(pbox)
00053         self.yline = QLineEdit(pbox)
00054         self.zline = QLineEdit(pbox)
00055 
00056         self.phi_line   = QLineEdit(pbox)
00057         self.theta_line = QLineEdit(pbox)
00058         self.psi_line   = QLineEdit(pbox)
00059 
00060         self.trans_vel_line = QLineEdit(pbox)
00061         self.rot_vel_line = QLineEdit(pbox)
00062 
00063         self.frameline  = QLineEdit(pbox)
00064         self.frameline.setText(self.default_frame)
00065         self.pose_button = QPushButton(pbox)
00066         self.pose_button.setText('Current Pose')
00067         self.rcommander.connect(self.pose_button, SIGNAL('clicked()'), self.get_current_pose)
00068 
00069         formlayout.addRow("&Arm", self.arm_box)
00070         formlayout.addRow("&Mode", self.motion_box)
00071         formlayout.addRow("&Point Input", self.source_box)
00072         formlayout.addRow("&x", self.xline)
00073         formlayout.addRow("&y", self.yline)
00074         formlayout.addRow("&z", self.zline)
00075 
00076         formlayout.addRow("&phi",   self.phi_line)
00077         formlayout.addRow("&theta", self.theta_line)
00078         formlayout.addRow("&psi",   self.psi_line)
00079 
00080         formlayout.addRow('&Translational Vel', self.trans_vel_line)
00081         formlayout.addRow('&Rotational Vel', self.rot_vel_line)
00082         formlayout.addRow("&frame", self.frameline)
00083         formlayout.addRow(self.pose_button)
00084         self.reset()
00085 
00086     def source_changed(self, index):
00087         self.source_box.setCurrentIndex(index)
00088         if str(self.source_box.currentText()) != ' ':
00089             self.xline.setEnabled(False)
00090             self.yline.setEnabled(False)
00091             self.zline.setEnabled(False)
00092             self.frameline.setEnabled(False)
00093             self.pose_button.setEnabled(False)
00094         else:
00095             self.xline.setEnabled(True)
00096             self.yline.setEnabled(True)
00097             self.zline.setEnabled(True)
00098             self.frameline.setEnabled(True)
00099             self.pose_button.setEnabled(True)
00100 
00101     def get_current_pose(self):
00102         frame_described_in = str(self.frameline.text())
00103         left = ('Left' == str(self.arm_box.currentText()))
00104         right = False
00105         if not left:
00106             right = True
00107             arm_tip_frame = LinearMoveTool.RIGHT_TIP
00108         else:
00109             arm_tip_frame = LinearMoveTool.LEFT_TIP
00110         
00111         self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.))
00112         p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))) * tr.identity_matrix()
00113         trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm)
00114 
00115         for value, vr in zip(trans, [self.xline, self.yline, self.zline]):
00116             vr.setText(str(value))
00117         for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]):
00118             vr.setText(str(np.degrees(value)))
00119 
00120     def _create_node(self, name=None):
00121         trans  = [float(vr.text()) for vr in [self.xline, self.yline, self.zline]]
00122         angles = [float(vr.text()) for vr in [self.phi_line, self.theta_line, self.psi_line]]
00123         frame  = str(self.frameline.text())
00124         trans_vel = float(str(self.trans_vel_line.text()))
00125         rot_vel   = float(str(self.rot_vel_line.text()))
00126         source_name = str(self.source_box.currentText())
00127         if source_name == ' ':
00128             source_name = None
00129 
00130         if name == None:
00131             nname = self.name + str(self.counter)
00132         else:
00133             nname = name
00134 
00135         return LinearMoveState(nname, trans, angles, 
00136                 str(self.arm_box.currentText()), [trans_vel, rot_vel],
00137                 str(self.motion_box.currentText()), source_name, frame)
00138 
00139         #state = NavigateState(nname, xy, theta, frame)
00140         #return state
00141 
00142     def _node_selected(self, node):
00143         for value, vr in zip(node.trans, [self.xline, self.yline, self.zline]):
00144             vr.setText(str(value))
00145         for value, vr in zip(node.angles, [self.phi_line, self.theta_line, self.psi_line]):
00146             vr.setText(str(value))
00147 
00148         self.frameline.setText(node.frame)
00149         self.motion_box.setCurrentIndex(self.motion_box.findText(str(node.motion_type)))
00150         self.arm_box.setCurrentIndex(self.arm_box.findText(node.arm))
00151 
00152         self.trans_vel_line.setText(str(node.vels[0]))
00153         self.rot_vel_line.setText(str(node.vels[1]))
00154 
00155         source_name = node.source_for('point')
00156         if source_name == None:
00157             source_name = ' '
00158             index = self.source_box.findText(source_name)
00159         else:
00160             index = self.source_box.findText(source_name)
00161             if index == -1:
00162                 self.source_box.addItem(source_name)
00163                 index = self.source_box.findText(source_name)
00164 
00165         print '>>>>>>>>>>>>>>>>> Source for linear node is', index
00166         self.source_changed(index)
00167 
00168     def reset(self):
00169         for vr in [self.xline, self.yline, self.zline]:
00170             vr.setText(str(0.0))
00171         for vr in [self.phi_line, self.theta_line, self.psi_line]:
00172             vr.setText(str(0.0))
00173         self.frameline.setText(self.default_frame)
00174         self.motion_box.setCurrentIndex(self.motion_box.findText('relative'))
00175         self.trans_vel_line.setText(str(.02))
00176         self.rot_vel_line.setText(str(.16))
00177 
00178     def get_smach_class(self):
00179         return LinearMoveState
00180 
00181 
00182 #
00183 # name maps to tool used to create it
00184 # model
00185 # is a state that can be stuffed into a state machine
00186 class LinearMoveState(tu.SimpleStateBase): # smach_ros.SimpleActionState):
00187     ##
00188     #
00189     # @param name
00190     # @param trans list of 3 floats
00191     # @param angles in euler list of 3 floats
00192     # @param frame 
00193     def __init__(self, name, trans, angles, arm, vels, motion_type, source, frame):
00194         tu.SimpleStateBase.__init__(self, name, \
00195                 arm + '_ptp', ptp.LinearMovementAction, 
00196                 goal_cb_str = 'ros_goal', input_keys=['point']) 
00197         self.set_source_for('point', source)
00198         #self.register_input_keys(['point'])
00199         #print 'registered input keys', self.get_registered_input_keys()
00200 
00201         self.trans = trans
00202         self.angles = angles #convert angles to _quat
00203         self.arm = arm
00204         self.vels = vels
00205         self.motion_type = motion_type
00206         self.frame = frame
00207 
00208     def ros_goal(self, userdata, default_goal):
00209         #print 'LinearMoveState: rosgoal called!!!!!!!!!!!!!!1'
00210         goal = ptp.LinearMovementGoal()
00211         if self.motion_type == 'relative':
00212             goal.relative = True
00213         elif self.motion_type == 'absolute':
00214             goal.relative = False
00215         else:
00216             raise RuntimeError('Invalid motion type given.')
00217 
00218         if self.source_for('point') != None:
00219             trans, frame = userdata.point
00220         else:
00221             trans = self.trans
00222             frame = self.frame
00223 
00224         pose = mat_to_pose(np.matrix(tr.translation_matrix(trans)) * np.matrix(tr.quaternion_matrix(self._quat)))
00225         goal.goal = stamp_pose(pose, frame)
00226         goal.trans_vel = self.vels[0]
00227         goal.rot_vel = self.vels[1]
00228         #print 'returned goal'
00229         return goal
00230 
00231     def _set_angles(self, euler_angs):
00232         ang_rad = [np.radians(e) for e in euler_angs]
00233         #self._quat = tr.quaternion_from_euler(euler_angs[0], euler_angs[1], euler_angs[2])
00234         self._quat = tr.quaternion_from_euler(*ang_rad)
00235     
00236     def _get_angles(self):
00237         return [np.degrees(e) for e in tr.euler_from_quaternion(self._quat)]
00238         
00239     angles = property(_get_angles, _set_angles)
00240 
00241     def __getstate__(self):
00242         state = tu.SimpleStateBase.__getstate__(self)
00243         my_state = [self.trans, self._quat, self.arm, self.vels, self.motion_type, self.frame]
00244         return {'simple_state': state, 'self': my_state}
00245 
00246     def __setstate__(self, state):
00247         tu.SimpleStateBase.__setstate__(self, state['simple_state'])
00248         self.trans, self._quat, self.arm, self.vels, self.motion_type, self.frame = state['self']
00249