def __init__(self, Tfm):
Thread.__init__(self)
self.trans, self.rot = conv.hmat_to_trans_rot(np.linalg.inv(Tfm))
self.transform_broadcaster = tf.TransformBroadcaster()
self.parent_frame = "/camera_depth_optical_frame"
self.child_frame = "/phasespace_frame"
self.time_period = 0.001
def run (self):
"""
Publishes the transforms stored.
"""
while True:
if self.publish_pc and self.cameras is not None:
data=self.cameras.get_RGBD()
for i,rgbd in data.items():
if rgbd['depth'] is None or rgbd['rgb'] is None:
print 'wut'
continue
camera_frame = 'camera%d_depth_optical_frame'%(i+1)
xyz = clouds.depth_to_xyz(rgbd['depth'], asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgbd['rgb'], camera_frame)
ar_cam = gmt.get_ar_marker_poses(None, None, pc=pc)
self.pc_pubs[i].publish(pc)
marker_frame = 'ar_marker%d_camera%d'
for marker in ar_cam:
trans, rot = conversions.hmat_to_trans_rot(ar_cam[marker])
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(),
marker_frame%(marker,i+1),
camera_frame)
if self.ready:
for parent, child in self.transforms:
trans, rot = self.transforms[parent, child]
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(),
child, parent)
time.sleep(1/self.rate)
def publish_transform_markers(grabber, marker, T, from_frame, to_frame, rate=100):
from visualization_msgs.msg import Marker
from sensor_msgs.msg import PointCloud2
print colorize("Transform : ", "yellow", True)
print T
marker_frame = "ar_frame"
tf_pub = tf.TransformBroadcaster()
pc_pub = rospy.Publisher("camera_points", PointCloud2)
trans = T[0:3,3]
rot = tf.transformations.quaternion_from_matrix(T)
sleeper = rospy.Rate(10)
while True:
try:
r, d = grabber.getRGBD()
ar_tfm = get_ar_transform_id (d, r, marker)
pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, from_frame)
pc_pub.publish(pc)
tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame)
try:
trans_m, rot_m = conversions.hmat_to_trans_rot(ar_tfm)
tf_pub.sendTransform(trans_m, rot_m, rospy.Time.now(), marker_frame, from_frame)
except:
pass
sleeper.sleep()
except KeyboardInterrupt:
break
def add_transforms(self, transforms):
"""
Takes a list of dicts with relevant transform information.
"""
if transforms is None:
return
for transform in transforms:
trans, rot = conversions.hmat_to_trans_rot(transform['tfm'])
self.transforms[transform['parent'],transform['child']] = (trans,rot)
self.ready = True
def publish_transform(T, from_frame, to_frame, rate=100):
print colorize("Transform : ", "yellow", True)
print T
tf_pub = tf.TransformBroadcaster()
trans, rot = conversions.hmat_to_trans_rot(T)
sleeper = rospy.Rate(10)
while True:
try:
tf_pub.sendTransform(trans, rot, rospy.Time.now(), to_frame, from_frame)
sleeper.sleep()
except KeyboardInterrupt:
break
def quick_calibrate(NUM_CAM, N_AVG):
rospy.init_node('quick_calibrate')
cameras = RosCameras(NUM_CAM)
tfm_pub = tf.TransformBroadcaster()
frames = {i:'/camera%i_link'%(i+1) for i in xrange(NUM_CAM)}
calib_tfms = {(frames[0],frames[i]):None for i in xrange(1,NUM_CAM)}
sleeper = rospy.Rate(30)
try:
while True:
tfms_found = {i:{} for i in xrange(NUM_CAM)}
for _ in xrange(N_AVG):
for i in xrange(NUM_CAM):
mtfms = cameras.get_ar_markers(camera=i)
for m in mtfms:
if m not in tfms_found[i]:
tfms_found[i][m] = []
tfms_found[i][m].append(mtfms[m])
for i in tfms_found:
for m in tfms_found[i]:
tfms_found[i][m] = utils.avg_transform(tfms_found[i][m])
for i in xrange(1,NUM_CAM):
rof_tfm = find_transform(tfms_found[0], tfms_found[i])
if rof_tfm is not None:
calib_tfms[(frames[0], frames[i])] = tfm_link_rof.dot(rof_tfm).dot(nlg.inv(tfm_link_rof))
for parent, child in calib_tfms:
if calib_tfms[parent, child] is not None:
trans, rot = conversions.hmat_to_trans_rot(calib_tfms[parent, child])
tfm_pub.sendTransform(trans, rot,
rospy.Time.now(),
child, parent)
sleeper.sleep()
except KeyboardInterrupt:
print "got ctrl-c"
def publish_gripper_tfms(self):
# self.langle_pub.publish(gmt.get_pot_angle('l'))
# self.rangle_pub.publish(gmt.get_pot_angle('r'))
transforms = []
for gr in self.grippers.values():
if self.gripper_lite:
transforms += gr.get_all_transforms(diff_cam=True)
else:
parent_frame = self.cameras.parent_frame
transforms += gr.get_all_transforms(parent_frame, diff_cam=True)
for transform in transforms:
(trans, rot) = conversions.hmat_to_trans_rot(transform['tfm'])
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(), transform['child'],
transform['parent'])
def visualize_ar ():
"""
Visualize point clouds from openni grabber and AR marker transforms through ROS.
"""
global getMarkers
if rospy.get_name() == '/unnamed':
rospy.init_node("visualize_ar")
camera_frame = "camera_depth_optical_frame"
tf_pub = tf.TransformBroadcaster()
pc_pub = rospy.Publisher("camera_points", PointCloud2)
sleeper = rospy.Rate(30)
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
grabber= cpr.CloudGrabber()
grabber.startRGBD()
print "Streaming now."
while True:
try:
r, d = grabber.getRGBD()
ar_tfms = get_ar_transform_id (d, r)
pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, camera_frame)
pc_pub.publish(pc)
for i in ar_tfms:
try:
trans, rot = conversions.hmat_to_trans_rot(ar_tfms[i])
tf_pub.sendTransform(trans, rot, rospy.Time.now(), "ar_marker_%d"%i, camera_frame)
except:
print "Warning: Problem with AR transform."
pass
sleeper.sleep()
except KeyboardInterrupt:
print "Keyboard interrupt. Exiting."
break
def publish_phasespace_markers_ros (print_shit=False):
import rospy
import roslib; roslib.load_manifest("tf")
import tf
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point
from std_msgs.msg import ColorRGBA
ph.turn_phasespace_on()
if rospy.get_name() == '/unnamed':
rospy.init_node("phasespace")
# ph.turn_phasespace_on()
marker_pub = rospy.Publisher('phasespace_markers', MarkerArray)
tf_pub = tf.TransformBroadcaster()
#prev_time = time.time() - 1/log_freq
while True:
try:
marker_pos = ph.get_marker_positions()
#print marker_pos#.keys()
time_stamp = rospy.Time.now()
mk = MarkerArray()
for i in marker_pos:
m = Marker()
m.pose.position.x = marker_pos[i][0]
m.pose.position.y = marker_pos[i][1]
m.pose.position.z = marker_pos[i][2]
m.pose.orientation.w = 1
m.id = i
m.header.stamp = time_stamp
m.header.frame_id = ph.PHASESPACE_FRAME
m.scale.x = m.scale.y = m.scale.z = 0.01
m.type = Marker.CUBE
m.color.r = 1
m.color.a = 1
mk.markers.append(m)
trans, rot = None, None
try:
trans, rot = conv.hmat_to_trans_rot(ph.marker_transform(0,1,2, marker_pos))
except:
print "Could not find phasespace transform."
pass
#curr_time = time.time()
#if curr_time - prev_time > 1/log_freq:
if print_shit: print marker_pos
marker_pub.publish(mk)
if trans is not None:
tf_pub.sendTransform(trans, rot, rospy.Time.now(), "ps_marker_transform", ph.PHASESPACE_FRAME)
# prev_time = curr_time
#sleep for remainder of the time
#time_passed = rospy.Time.now().to_sec() - time_stamp.to_sec()
#time.sleep(0.2)
#time.sleep(max(1/log_freq-time_passed,0))
except KeyboardInterrupt:
break
ph.turn_phasespace_off()
sleeper.sleep()
if __name__ == '__main__':
rospy.init_node('calib_hydra_pr2')
parser = argparse.ArgumentParser(description="Hydra Kinect Calibration")
parser.add_argument('--arm', help="in {'right', 'left'} : the pr2 arm to track.", required=True)
parser.add_argument('--hydra', help="in {'right', 'left'} : the hydra handle to track.", required=True)
parser.add_argument('--n_tfm', help="number of transforms to use for calibration.", type=int, default=5)
parser.add_argument('--n_avg', help="number of estimates of transform to use for averaging.", type=int, default=5)
parser.add_argument('--publish_tf', help="whether to publish the transform between hydra_base and camera_link", default=True)
vals = parser.parse_args()
arm_tfms, hydra_tfms = get_transforms(vals.arm, vals.hydra, vals.n_tfm, vals.n_avg)
if vals.publish_tf:
T_ms = solve_sylvester4(arm_tfms, hydra_tfms)
T_chs = [arm_tfms[i].dot(T_ms).dot(np.linalg.inv(hydra_tfms[i])) for i in xrange(len(arm_tfms))]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_chs]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans,axis=0)/trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
T_ch = conversions.trans_rot_to_hmat(avg_trans, avg_rot)
print T_ch
publish_tf(T_ch, 'base_footprint', 'hydra_base')
#subprocess.call("sudo killall XnSensorServer", shell=True)
device.open()
colorStream = device.createStream("color", width=640, height=480, fps=30)
colorStream.start()
depthStream = device.createStream("depth", width=640, height = 480, fps=30)
depthStream.start()
print "Streaming now."
while True:
try:
r = colorStream.readFrame().data
d = depthStream.readFrame().data
ar_tfm = get_ar_transform_id (d, r, 2)
print ar_tfm
#pc = ru.xyzrgb2pc(clouds.depth_to_xyz(d, asus_xtion_pro_f), r, camera_frame)
#pc_pub.publish(pc)
try:
trans, rot = conversions.hmat_to_trans_rot(ar_tfm)
tf_pub.sendTransform(trans, rot, rospy.Time.now(), "ar_marker_%d"%i, camera_frame)
except:
print "Warning: Problem with AR transform."
pass
sleeper.sleep()
except KeyboardInterrupt:
print "Keyboard interrupt. Exiting."
break
def get_transforms(arm, hydra, n_tfm , n_avg):
"""
Returns a tuple of two list of N_TFM transforms, each
the average of N_AVG transforms
corresponding to the left/ right arm of the pr2 and
the hydra paddle of the HYDRA ('right' or 'left') side.
Return gripper_tfms, hydra_tfms, kinect_tfms, head_tfms
"""
pr2_frame = 'base_footprint'
assert arm == 'right' or 'left'
arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm]
head_frame = 'head_plate_frame'
hydra_frame = 'hydra_base'
assert hydra =='right' or hydra=='left'
paddle_frame = 'hydra_%s' % hydra
tf_sub = tf.TransformListener()
I_0 = np.eye(4)
I_0[3, 3] = 0
gripper_tfms = []
hydra_tfms = []
kinect_tfms = []
head_tfms = []
ar_markers = ARMarkersRos('/camera1_')
time.sleep(3)
marker_id = 11
i = 0
while i < n_tfm:
raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm-1), "red", True))
## transforms which need to be averaged.
gtfms = []
htfms = []
ktfms = []
ptfms = []
sleeper = rospy.Rate(30)
collect_enough_data = True
for j in xrange(n_avg):
print colorize('\tGetting averaging transform : %d of %d ...'%(j, n_avg-1), "blue", True)
kinect_tfm = ar_markers.get_marker_transforms(time_thresh=0.5)
print kinect_tfm
if kinect_tfm == {}:
print "Lost sight of AR marker. Breaking..."
collect_enough_data = False
break
ptrans, prot = tf_sub.lookupTransform(pr2_frame, head_frame, rospy.Time(0))
ptfms.append(conversions.trans_rot_to_hmat(ptrans,prot))
gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0))
gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot))
htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
htfms.append(conversions.trans_rot_to_hmat(htrans, hrot))
ktrans, krot = conversions.hmat_to_trans_rot(kinect_tfm[marker_id])
ktfms.append(conversions.trans_rot_to_hmat(ktrans, krot))
sleeper.sleep()
if collect_enough_data:
gripper_tfms.append(avg_transform(gtfms))
hydra_tfms.append(avg_transform(htfms))
kinect_tfms.append(avg_transform(ktfms))
head_tfms.append(avg_transform(ptfms))
i += 1
return (gripper_tfms, hydra_tfms, kinect_tfms, head_tfms)
parser = argparse.ArgumentParser(description="Hydra Kinect Calibration")
parser.add_argument('--arm', help="in {'right', 'left'} : the pr2 arm to track.", required=True)
parser.add_argument('--hydra', help="in {'right', 'left'} : the hydra handle to track.", required=True)
parser.add_argument('--n_tfm', help="number of transforms to use for calibration.", type=int, default=5)
parser.add_argument('--n_avg', help="number of estimates of transform to use for averaging.", type=int, default=5)
parser.add_argument('--publish_tf', help="whether to publish the transform between hydra_base and camera_link", default=True)
vals = parser.parse_args()
arm_tfms, hydra_tfms, kinect_tfms, head_tfms = get_transforms(vals.arm, vals.hydra, vals.n_tfm, vals.n_avg)
if vals.publish_tf:
# Solving for transform between base_footprint and hydra_base
T_ms = ss.solve4(arm_tfms, hydra_tfms)
T_chs = [arm_tfms[i].dot(T_ms).dot(np.linalg.inv(hydra_tfms[i])) for i in xrange(len(arm_tfms))]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_chs]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans, axis=0) / trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
T_ch = conversions.trans_rot_to_hmat(avg_trans, avg_rot)
# Average transform from gripper to hydra (T_gh)
T_grip_hydra = [np.linalg.inv(arm_tfms[i]).dot(T_ch).dot(hydra_tfms[i]) for i in xrange(vals.n_tfm)]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_grip_hydra]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans, axis=0) / trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
parser.add_argument('--publish_tf', help="whether to publish the transform between hydra_base and camera_link", default=True)
vals = parser.parse_args()
#tool_tfms, model_tfms = get_tool_model(vals.n_tfm, vals.n_avg)
tool_tfms = get_tool_model(vals.n_tfm, vals.n_avg)
tool_to_model_tfms = []
for i in xrange(vals.n_tfm):
tool = tool_tfms[i]
model = model_tfms[i]
tool_inv = nlg.inv(tool)
tool_to_model_tfm = tool_inv.dot(model)
tool_to_model_tfms.append(tool_to_model_tfm)
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in tool_to_model_tfms]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans,axis=0)/trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
tool_to_model_tfm_avg = conversions.trans_rot_to_hmat(avg_trans, avg_rot)
print tool_to_model_tfm_avg
if vals.publish_tf:
publish_tf(tool_to_model_tfm_avg, 'l_gripper_tool_frame', 'gripper_model_frame')
rospy.init_node('calib_hydra_pr2')
parser = argparse.ArgumentParser(description="Hydra Kinect Calibration")
parser.add_argument('--arm', help="in {'right', 'left'} : the pr2 arm to track.", required=True)
parser.add_argument('--hydra', help="in {'right', 'left'} : the hydra handle to track.", required=True)
parser.add_argument('--n_tfm', help="number of transforms to use for calibration.", type=int, default=5)
parser.add_argument('--n_avg', help="number of estimates of transform to use for averaging.", type=int, default=5)
parser.add_argument('--publish_tf', help="whether to publish the transform between hydra_base and camera_link", default=True)
vals = parser.parse_args()
arm_tfms, hydra_tfms = get_transforms(vals.arm, vals.hydra, vals.n_tfm, vals.n_avg)
if vals.publish_tf:
T_ms = ss.solve4(arm_tfms, hydra_tfms)
T_chs = [arm_tfms[i].dot(T_ms).dot(np.linalg.inv(hydra_tfms[i])) for i in xrange(len(arm_tfms))]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_chs]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans, axis=0) / trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
T_ch = conversions.trans_rot_to_hmat(avg_trans, avg_rot)
T_arm_sensor = [np.linalg.inv(arm_tfms[i]).dot(T_ch).dot(hydra_tfms[i]) for i in xrange(len(arm_tfms))]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_arm_sensor]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans, axis=0) / trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
T_as = conversions.trans_rot_to_hmat(avg_trans, avg_rot)
while(i < args.n_tfm):
tfm1 = None
tfm2 = None
rgb1 = cs1.readFrame()
#cv2.imshow("rgb", rgb.data)
depth1 = ds1.readFrame()
#cv2.imshow("depth", cmap[np.fmin((depth.data*.064).astype('int'), 255)])
tfm1 = get_ar_transform_id(depth1.data, rgb1.data, 2)
print tfm1
if tfm1 == None:
print "got none"
continue
#print tfm
#tfms1.append(tfm1)
k1_t, k1_q = conversions.hmat_to_trans_rot(tfm1)
k1_ts = np.r_[k1_ts, np.array(k1_t, ndmin=2)]
k1_qs = np.r_[k1_qs, np.array(k1_q, ndmin=2)]
rgb2 = cs2.readFrame()
depth2 = ds2.readFrame()
tfm2 = get_ar_transform_id(depth2.data, rgb2.data, 2)
#print tfm
#trans, rot = conversions.hmat_to_trans_rot(tfm)
print tfm2
if tfm2 == None:
print "got none"
continue
k2_t, k2_q = conversions.hmat_to_trans_rot(tfm2)
k2_ts = np.r_[k2_ts, np.array(k2_t, ndmin=2)]
k2_qs = np.r_[k2_qs, np.array(k2_q, ndmin=2)]
