rcommander: pointcloud_click_tool.py Source File

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 import geometry_msgs.msg as geo
00006 import smach
00007 import rospy
00008 import tf.transformations as tr
00009 from tf_broadcast_server.srv import BroadcastTransform, GetTransforms, ClearTransforms
00010 #from object_manipulator.convert_functions import mat_to_pose, stamp_pose, change_pose_stamped_frame
00011 import tf_utils as tfu
00012 
00013 
00014 class PointCloudClickTool(tu.ToolBase):
00015 
00016     def __init__(self, rcommander):
00017         tu.ToolBase.__init__(self, rcommander, 'create_origin', 'Create Origin', Point3DState)
00018         self.default_frame = '/base_link'
00019         self.tf_listener = rcommander.tf_listener
00020         self.frames_service = rospy.ServiceProxy('get_transforms', GetTransforms)
00021         self.clear_frames_service = rospy.ServiceProxy('clear_all_transforms', ClearTransforms)
00022 
00023     def fill_property_box(self, pbox):
00024         formlayout = pbox.layout()
00025 
00026         self.xline = QLineEdit(pbox)
00027         self.yline = QLineEdit(pbox)
00028         self.zline = QLineEdit(pbox)
00029 
00030         self.time_out_box = QDoubleSpinBox(pbox)
00031         self.time_out_box.setMinimum(1.)
00032         self.time_out_box.setMaximum(1000.)
00033         self.time_out_box.setSingleStep(1.)
00034 
00035         self.frame_box = tu.FrameBox(self.frames_service)
00036         #self.frame_box = QComboBox(pbox)
00037         #self.orientation_frame_box = QComboBox(pbox)
00038         self.orientation_frame_box = tu.FrameBox(self.frames_service)
00039         #for f in self.tf_listener.getFrameStrings():
00040         #for f in self.frames_service().frames:
00041             #self.frame_box.addItem(f)
00042             #self.orientation_frame_box.addItem(f)
00043 
00044         self.pose_button = QPushButton(pbox)
00045         self.pose_button.setText('Get Point')
00046 
00047         self.clear_frames_button = QPushButton(pbox)
00048         self.clear_frames_button.setText('Clear Frames')
00049 
00050         self.wait_check = QCheckBox(pbox)
00051         self.wait_check.setTristate(False)
00052 
00053         pos_group = QGroupBox('Position', pbox)
00054         pos_layout = QFormLayout(pos_group)
00055         pos_group.setLayout(pos_layout)
00056         pos_layout.addRow("&Frame", self.frame_box.create_box(pbox))
00057         pos_layout.addRow("&X", self.xline)
00058         pos_layout.addRow("&Y", self.yline)
00059         pos_layout.addRow("&Z", self.zline)
00060         formlayout.addRow(pos_group)
00061 
00062         ori_group = QGroupBox("Orientation", pbox)
00063         ori_layout = QFormLayout(ori_group)
00064         ori_layout.addRow("Frame", self.orientation_frame_box.create_box(pbox))
00065         formlayout.addRow(ori_group)
00066 
00067         formlayout.addRow('Wait For Point', self.wait_check)
00068         formlayout.addRow('Wait Time Out', self.time_out_box)
00069         formlayout.addRow(self.pose_button)
00070         formlayout.addRow(self.clear_frames_button)
00071 
00072         self.rcommander.connect(self.pose_button, SIGNAL('clicked()'), self.get_current_pose)
00073         self.rcommander.connect(self.wait_check, SIGNAL('stateChanged(int)'), self.box_checked)
00074         self.rcommander.connect(self.clear_frames_button, SIGNAL('clicked()'), self.clear_frames_button_cb)
00075         
00076         self.reset()
00077         pbox.update()
00078 
00079     def clear_frames_button_cb(self):
00080         self.clear_frames_service()
00081 
00082     def new_node(self, name=None):
00083         point = [float(str(self.xline.text())), float(str(self.yline.text())), float(str(self.zline.text()))]
00084         #point_frame = str(self.frame_box.currentText())
00085         point_frame = self.frame_box.text()
00086         #orientation_frame = str(self.orientation_frame_box.currentText())
00087         orientation_frame = self.orientation_frame_box.text()
00088         if name == None:
00089             nname = self.name + str(self.counter)
00090         else:
00091             nname = name
00092 
00093         wait_for_msg = self.wait_check.isChecked()
00094         time_out = self.time_out_box.value()
00095         return Point3DState(nname, point, #angle, 
00096                             point_frame, orientation_frame, 
00097                             wait_for_msg=wait_for_msg, time_out=time_out)
00098 
00099     def box_checked(self, state):
00100         if state == 0:
00101             self.xline.setEnabled(True)
00102             self.yline.setEnabled(True)
00103             self.zline.setEnabled(True)
00104 
00105             self.frame_box.setEnabled(True)
00106             self.pose_button.setEnabled(True)
00107             
00108         if state == 2:
00109             self.xline.setEnabled(False)
00110             self.yline.setEnabled(False)
00111             self.zline.setEnabled(False)
00112 
00113             self.frame_box.setEnabled(False)
00114             self.pose_button.setEnabled(False)
00115 
00116     def set_node_properties(self, node):
00117         self.xline.setText(str(node.point[0]))
00118         self.yline.setText(str(node.point[1]))
00119         self.zline.setText(str(node.point[2]))
00120         self.frame_box.set_text(node.point_frame)
00121         self.orientation_frame_box.set_text(node.orientation_frame)
00122         self.time_out_box.setValue(node.time_out)
00123         if node.wait_for_msg:
00124             self.wait_check.setCheckState(2)
00125         #self.frame_box.setCurrentIndex(self.frame_box.findText(str(node.point_frame)))
00126 
00127     def reset(self):
00128         self.xline.setText(str(0.))
00129         self.yline.setText(str(0.))
00130         self.zline.setText(str(0.))
00131 
00132         self.time_out_box.setValue(120.)
00133         self.wait_check.setCheckState(False)
00134         self.frame_box.set_text(self.default_frame, create=False)
00135         self.orientation_frame_box.set_text(self.default_frame, create=False)
00136         #self.frame_box.setCurrentIndex(self.frame_box.findText(self.default_frame))
00137 
00138     def get_current_pose(self):
00139         pose_stamped = rospy.wait_for_message('/cloud_click_point', geo.PoseStamped, 5.)
00140         self.xline.setText('%.3f' % pose_stamped.pose.position.x)
00141         self.yline.setText('%.3f' % pose_stamped.pose.position.y)
00142         self.zline.setText('%.3f' % pose_stamped.pose.position.z)
00143 
00144         q = pose_stamped.pose.orientation
00145         phi, theta, psi = tr.euler_from_quaternion([q.x, q.y, q.z, q.w])
00146         self.psi_line.setText('%.3f' % phi)
00147         self.theta_line.setText('%.3f' % theta)
00148         self.psi_line.setText('%.3f' % psi)
00149         self.frame_box.set_text(pose_stamped.header.frame_id)
00150         self.orientation_frame_box.set_text(pose_stamped.header.frame_id)
00151         #idx = tu.combobox_idx(self.frame_box, pose_stamped.header.frame_id)
00152         #self.frame_box.setCurrentIndex(idx)
00153 
00154 class WaitForMessage:
00155 
00156     def __init__(self, topic, message_type):
00157         self.subscriber = rospy.Subscriber(topic, message_type, self.message_cb)
00158         self.msg = None
00159 
00160     def message_cb(self, message):
00161         self.msg = message
00162 
00163     def get_message(self):
00164         return self.msg
00165 
00166     def __del__(self):
00167         self.subscriber.unregister()
00168 
00169 
00170 class Point3DState(tu.StateBase):
00171 
00172     def __init__(self, name, point, #angle, 
00173                 point_frame, orientation_frame, wait_for_msg, time_out):
00174         tu.StateBase.__init__(self, name)
00175         self.point = point
00176         self.point_frame = point_frame
00177         self.orientation_frame = orientation_frame
00178         self.time_out = time_out
00179         self.wait_for_msg = wait_for_msg
00180 
00181     def get_smach_state(self):
00182         if self.wait_for_msg:
00183             return Point3DStateSmach(self.get_name(), self.orientation_frame, message = None, time_out = self.time_out)
00184         else:
00185             ps = geo.PointStamped()
00186             ps.header.frame_id = self.point_frame
00187             ps.header.stamp = rospy.Time.now()
00188             ps.point.x = self.point[0]
00189             ps.point.y = self.point[1]
00190             ps.point.z = self.point[2]
00191 
00192             return Point3DStateSmach(self.get_name(), self.orientation_frame, message = ps, time_out = None)
00193 
00194 
00195 class Point3DStateSmach(smach.State):
00196 
00197     ##
00198     #@param message if self.message is None we will wait for a message, else use provided message
00199     #@param time_out if we have to wait for a message specify how long to wait before timing out
00200     def __init__(self, frame_name, orientation_frame, message = None, time_out = None):
00201         smach.State.__init__(self, 
00202                 outcomes = ['succeeded', 'preempted', 'timed_out'], 
00203                 input_keys = [], output_keys = [])
00204 
00205         self.frame_name = frame_name
00206         self.orientation_frame = orientation_frame
00207         self.time_out = time_out
00208         self.message = message
00209         self.broadcast_transform_srv = rospy.ServiceProxy('broadcast_transform', BroadcastTransform)
00210 
00211     def set_robot(self, robot):
00212         self.robot = robot
00213 
00214     def execute(self, userdata):
00215         #Listen for a point if we don't have one.  
00216         point_stamped = self.message
00217         t_start = rospy.get_time()
00218         r = rospy.Rate(10)
00219         waitobj = WaitForMessage('/cloud_click_point', geo.PoseStamped)
00220         pose_stamped = None
00221         while point_stamped == None:
00222             try:
00223                 pose_stamped = waitobj.get_message()
00224                 if pose_stamped != None:
00225                     point_stamped = geo.PointStamped()
00226                     point_stamped.header = pose_stamped.header
00227                     point_stamped.point = pose_stamped.pose.position
00228                 
00229                 r.sleep()
00230             except rospy.ROSException, e:
00231                 pass
00232 
00233             if self.preempt_requested():
00234                 self.service_preempt()
00235                 return 'preempted'
00236 
00237             if (self.time_out != None) and ((rospy.get_time() - t_start) > self.time_out):
00238                 return 'timed_out'
00239         del waitobj
00240 
00241         # The given point transforms from this NEW FRAME (self.frame_name) 
00242         # to original parent frame (header.frame_id).
00243         frame_id_T_frame_name = tfu.origin_to_frame(point_stamped, self.orientation_frame, 
00244                 self.robot.tf_listener, point_stamped.header.frame_id)
00245         pose = tfu.stamp_pose(tfu.mat_to_pose(frame_id_T_frame_name), point_stamped.header.frame_id)
00246         #self.broadcast_transform_srv(self.frame_name, pose)
00247         #print 'using new version'
00248         #pose_stamped = change_pose_stamped_frame(self.robot.tf_listener, pose_stamped, '/base_link')
00249         self.broadcast_transform_srv(self.frame_name, pose)
00250 
00251 
00252         return 'succeeded'
00253 
00254 
00255 
00256