def get_device (device_id):
"""
Takes in a string @device_id. If device_id is
"""
devices = cyni.enumerateDevices()
if len(devices) == 0:
print "No devices found! Maybe you forgot to call cyni.initialize()"
return None
if device_id[0] == "#":
try:
num = int(device_id[1:])
if num > len(devices):
print "Index out of range."
return None
else: return cyni.Device(devices[num-1]['uri'])
except:
print "Incorrect device_id %s"%device_id
return None
else:
device_uris = [device['uri'] for device in devices]
if device_id not in device_uris:
print "Device with URI %s not found."%device_id
return None
else: return cyni.Device(device_id)
def __init__(self, num_cameras=2):
assert num_cameras > 0
# Corresponds to camera 1
self.parent_frame = 'camera1_depth_optical_frame'
if rospy.get_name() == '/unnamed':
rospy.init_node('cam_calibrator')
self.tf_listener = tf.TransformListener()
cyni.initialize()
self.allDevices = cyni.enumerateDevices()
if len(self.allDevices) == 0:
raise Exception("No devices found! Cyni not initialized properly or, devices actually not present.")
if self.num_cameras > len(self.allDevices):
redprint("Warning: Requesting more devices than available. Getting all devices.")
self.num_cameras = min(num_cameras,len(self.allDevices))
#subprocess.call("sudo killall XnSensorServer", shell=True)
cmap = np.zeros((256, 3),dtype='uint8')
cmap[:,0] = range(256)
cmap[:,2] = range(256)[::-1]
cmap[0] = [0,0,0]
#grabber = cloudprocpy.CloudGrabber("#1")
#grabber.startRGBD()
#g1 = cloudprocpy.CloudGrabber("#1")
#g1.startRGBD()
cyni.initialize()
device_list = cyni.enumerateDevices()
d1 = cyni.Device(device_list[0]['uri'])
d2 = cyni.Device(device_list[1]['uri'])
d1.open()
d2.open()
#colorStream = d1.createStream("color", width=640, height=480, fps=30)
#colorStream.start()
ds1 = d1.createStream("depth", width=640, height = 480, fps=30)
ds1.start()
ds2 = d2.createStream("depth", width=640, height = 480, fps = 30)
ds2.start()
emitterControl = False
try:
while True:
#rgb = colorStream.readFrame()
