Go to the documentation of this file.00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import sensor_msgs.msg as sm
00004 import hrl_lib.rutils as ru
00005 import numpy as np
00006 import pr2_msgs.msg as pm
00007 import tf
00008 import hrl_lib.tf_utils as tfu
00009 import tf.transformations as tr
00010 import time
00011 import hrl_lib.util as ut
00012
00013 def pointcloud_to_np(pc):
00014 plist = []
00015 for p in pc.points:
00016 plist.append([p.x, p.y, p.z])
00017 return np.matrix(plist).T
00018
00019
00020 class ContactTipLocation:
00021 def __init__(self):
00022 rospy.init_node('contact3d')
00023 rospy.Subscriber('/pressure/l_gripper_motor', pm.PressureState, self.lpress_cb)
00024 rospy.Subscriber('/pressure/l_gripper_motor', pm.PressureState, self.rpress_cb)
00025 self.ftip_frames = ['r_gripper_l_finger_tip_link',
00026 'r_gripper_r_finger_tip_link',
00027 'l_gripper_l_finger_tip_link',
00028 'l_gripper_r_finger_tip_link']
00029
00030 self.tflistener = tf.TransformListener()
00031 self.lmat0 = None
00032 self.rmat0 = None
00033 self.contact_locs = []
00034 self.last_msg = None
00035
00036 def lpress_cb(self, pmsg):
00037
00038 lmat = np.matrix((pmsg.l_finger_tip)).T
00039 rmat = np.matrix((pmsg.r_finger_tip)).T
00040 if self.lmat0 == None:
00041 self.lmat0 = lmat
00042 self.rmat0 = rmat
00043 return
00044
00045
00046 lmat = lmat - self.lmat0
00047 rmat = rmat - self.rmat0
00048
00049
00050 if np.any(np.abs(lmat) > 250) or np.any(np.abs(rmat) > 250):
00051
00052 to_frame = 'base_link'
00053 def frame_loc(from_frame):
00054 p_base = tfu.transform('base_footprint', from_frame, self.tflistener) \
00055 * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
00056
00057 return tfu.matrix_as_tf(p_base)
00058
00059 tip_locs = [frame_loc(n)[0] for n in self.ftip_frames]
00060 t = pmsg.header.stamp.to_time()
00061 rospy.loginfo("contact detected at %.3f" % t)
00062 self.contact_locs.append([t, tip_locs])
00063
00064 self.last_msg = time.time()
00065 rospy.loginfo('lpress_cb ' + str(np.max(rmat)) + ' ' + str(np.max(lmat)))
00066
00067 def rpress_cb(self, pmsesg):
00068 pass
00069
00070
00071
00072 if __name__ == '__main__':
00073 import sys
00074 import pdb
00075
00076 fname = sys.argv[1]
00077 scene = None
00078
00079
00080
00081
00082 i = 0
00083 for top, pc, t in ru.bag_iter(fname, ['/full_cloud']):
00084
00085 if len(pc.points) > 20000:
00086 if i > 0:
00087 pdb.set_trace()
00088 scene = pointcloud_to_np(pc)
00089 break
00090 i = i + 1
00091
00092
00093
00094 ctl = ContactTipLocation()
00095 r = rospy.Rate(10)
00096 print 'running contact tip recorder'
00097 while not rospy.is_shutdown():
00098 r.sleep()
00099 if ctl.last_msg != None and (time.time() - ctl.last_msg) > 60.0:
00100 break
00101
00102 print 'saving pickle contact_locs.pkl'
00103 ut.save_pickle([scene, ctl.contact_locs], 'contact_locs.pkl')
00104
00105 pdb.set_trace()
00106 print 'done.'
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144