def save_clouds():
NUM_CAM = 2
cameras = RosCameras(NUM_CAM)
tfm_listener = tf.TransformListener()
fr1 = camera1_rgb_optical_frame
fr2 = camera2_rgb_optical_frame
c1, c2 = 0, 1
print "Waiting for service .."
rospy.wait_for_service('pcd_service')
pcdService = rospy.ServiceProxy("pcd_service", PCDData)
raw_input(colorize("Do not move the objects on the table now.", "green", True))
raw_input(colorize("Cover camera %i and hit enter!" % (c2 + 1), 'yellow', True))
pc1 = cameras.get_pointcloud(c1)
raw_input(colorize("Cover camera %i and hit enter!" % (c1 + 1), 'yellow', True))
pc2 = cameras.get_pointcloud(c2)
pc2 = ru.transformPointCloud2(pc2, tfm_listener, fr1, fr2)
req = PCDDataRequest()
req.pc = pc1
req.fname = "cloud1.pcd"
pcdService(req)
req.pc = pc2
req.fname = "cloud2.pcd"
pcdService(req)
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"
