def start():
global XAXIS
global YAXIS
global TEST_CASES
global DIRECTION
global GLOBAL_DEPTH_MAP
plt.ion()
plt.figure()
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
DIRECTION = 0
for i in xrange(5):
initial_map = get_depth()
while True:
GLOBAL_DEPTH_MAP = get_depth() #returns the depth frame
back_movement(GLOBAL_DEPTH_MAP)
contoursright = contours_return(GLOBAL_DEPTH_MAP, -10)
contoursleft = contours_return(GLOBAL_DEPTH_MAP, 10)
door_detection(contoursright, contoursleft)
if DOOR_FLAG:
door_movement()
else:
regular_movement()
cv2.imshow('final', GLOBAL_DEPTH_MAP)
if cv2.waitKey(1) != -1:
SERIALDATA.write('\x35')
SERIALDATA.close()
break
cv2.destroyAllWindows()
def loop(processimg):
if not use_webcam:
ctx = freenect.init()
dev = freenect.open_device(ctx, 0)
freenect.set_tilt_degs(dev, 10)
freenect.close_device(dev)
cv2.namedWindow('processing')
for k, v in params.iteritems():
cv2.createTrackbar(k, 'processing', v, 255, onchange(k))
runonce = True
while runonce:
#runonce = False
if imgpath != "":
img = cv2.imread(imgpath)
else:
img = getimg()
cv2.imshow('processing', cv2.resize(processimg(img), (width, height)))
char = cv2.waitKey(10)
if char == 27:
break
elif char == ord('p'):
for k, v in params.iteritems():
print "%s: %d" % (k, v)
#freenect.set_tilt_degs(dev, pid)
cv2.destroyAllWindows()
def start():
ser = serial.Serial(
'/dev/ttyUSB0') #initialization of serial communication
global test_cases, ser, global_depth_map, DOOR_FLAG, DOOR_COUNT
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
test_cases = [True, True, True]
while True:
global_depth_map = get_depth() #returns the depth frame
contoursright = contours_return(global_depth_map, -10)
contoursleft = contours_return(global_depth_map, 10)
door_detection(contoursright, contoursleft, test_cases)
if DOOR_FLAG:
door_movement(global_depth_map)
else:
regular_movement(global_depth_map)
cv2.imshow('final', global_depth_map)
if cv2.waitKey(1) != -1:
ser.write('\x35')
ser.close()
break
cv2.destroyAllWindows()
def start():
global XAXIS
global YAXIS
global TEST_CASES
global DIRECTION
global GLOBAL_DEPTH_MAP
plt.ion()
plt.figure()
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
DIRECTION = 0
for i in xrange(5):
initial_map = get_depth()
while True:
GLOBAL_DEPTH_MAP = get_depth() #returns the depth frame
back_movement(GLOBAL_DEPTH_MAP)
contoursright = contours_return(GLOBAL_DEPTH_MAP, -10)
contoursleft = contours_return(GLOBAL_DEPTH_MAP, 10)
door_detection(contoursright, contoursleft)
if DOOR_FLAG:
door_movement()
else: regular_movement()
cv2.imshow('final', GLOBAL_DEPTH_MAP)
if cv2.waitKey(1) != -1:
SERIALDATA.write('\x35')
SERIALDATA.close()
break
cv2.destroyAllWindows()
def body(dev, ctx):
global tilt
global light
global increment
global last_time
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
if tilt >= 30:
increment = False
if tilt <= 10:
increment = True
led = light
freenect.set_led(dev, led)
if search:
if increment:
tilt = tilt + 5
else:
tilt = tilt - 5
freenect.set_tilt_degs(dev, tilt)
def body(dev,ctx):
if sys.argv < 2:
exit(0);
global keep_running
global myAngle
global k
global image
char_set = string.ascii_uppercase + string.digits
k = (cv.WaitKey(10) & 255)
if k == 27:
print "Exiting..."
keep_running = False
elif chr(k) == 'w' or chr(k) == 'W':
myAngle = myAngle + 5
elif chr(k) == 's' or chr(k) == 'S':
myAngle = myAngle - 5
elif chr(k) == 'p' or chr(k) == 'P':
cv.SaveImage('positive/pos-'+ rand_string(10) + '.jpg',image)
print "Positive image saved"
elif chr(k) == 'n' or chr(k) == 'N':
cv.SaveImage('negative/neg-'+ rand_string(10) + '.jpg',image)
print "Negative image saved"
freenect.set_led(dev,1)
freenect.set_tilt_degs(dev,myAngle)
if not keep_running:
freenect.set_led(dev,0)
raise freenect.Kill
def loop(processimg):
if not use_webcam:
ctx = freenect.init()
dev = freenect.open_device(ctx, 0)
freenect.set_tilt_degs(dev, 10)
freenect.close_device(dev)
cv2.namedWindow('processing')
for k, v in params.iteritems():
cv2.createTrackbar(k, 'processing', v, 255, onchange(k))
runonce = True
while runonce:
#runonce = False
if imgpath != "":
img = cv2.imread(imgpath)
else:
img = getimg()
cv2.imshow('processing', cv2.resize(processimg(img), (width, height)))
char = cv2.waitKey(10)
if char == 27:
break
elif char == ord('p'):
for k, v in params.iteritems():
print "%s: %d" % (k, v)
#freenect.set_tilt_degs(dev, pid)
cv2.destroyAllWindows()
def start():
ser = serial.Serial('/dev/ttyUSB0') #initialization of serial communication
global test_cases, ser, global_depth_map, DOOR_FLAG, DOOR_COUNT
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
test_cases = [True, True, True]
while True:
global_depth_map = get_depth() #returns the depth frame
contoursright = contours_return(global_depth_map, -10)
contoursleft = contours_return(global_depth_map, 10)
door_detection(contoursright, contoursleft, test_cases)
if DOOR_FLAG:
door_movement(global_depth_map)
else: regular_movement(global_depth_map)
cv2.imshow('final', global_depth_map)
if cv2.waitKey(1) != -1:
ser.write('\x35')
ser.close()
break
cv2.destroyAllWindows()
def set_tilt(deg):
#ctx = freenect.init()
#mdev = freenect.open_device(ctx, 0)
freenect.set_tilt_degs(mdev, deg)
#freenect.close_device(mdev)
#freenect.shutdown(ctx)
print("Tilt Degree: {}".format(deg))
def body(self, dev, ctx):
if self.killfreenect:
raise freenect.Kill("Killing freenect runloop")
if self.server.tilt is None or self.server.tilt == self.last_tilt:
return
else:
freenect.set_tilt_degs(dev, self.server.tilt)
self.last_tilt = self.server.tilt
def body(self, dev, ctx):
if self.killfreenect:
raise freenect.Kill("Killing freenect runloop")
if self.server.tilt is None or self.server.tilt == self.last_tilt:
return
else:
freenect.set_tilt_degs(dev, self.server.tilt)
self.last_tilt = self.server.tilt
def body(dev, ctx):
global last_time
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def body(dev, ctx):
global last_time
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def rotate():
led = 0
for i in range(31):
freenect.set_tilt_degs(new_dev, i)
print(f"iterataion number {i}")
time.sleep(0.1)
if i % 5 == 0:
freenect.set_led(new_dev, led)
led += 1
def body(dev, ctx):
global last_time
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
def start(self, degs=10):
self.ctx = freenect.init()
if not self.ctx:
freenect.error_open_device()
self.dev = freenect.open_device(self.ctx, 0)
if not self.dev:
freenect.error_open_device()
freenect.set_tilt_degs(self.dev, -degs)
freenect.set_tilt_degs(self.dev, degs)
self.intialised == True
print('kinect Started')
def body(dev, ctx):
global last_time
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
def __init__(self, calibration_file='config/calibration.frame', debug=False):
try:
self.device = kinect.open_device(kinect.init(), 0)
kinect.set_tilt_degs(self.device, -90)
kinect.close_device(self.device)
except Exception as e:
logging.error('No Kinect detected')
self.device = None
with open(calibration_file) as calibration_file:
self.calibration_frame = np.load(calibration_file)
'''
def body(dev, ctx):
#global last_time
if not keep_running:
raise freenect.Kill
#last_time = time.time()
led = 1
tilt = int(input("Enter a degree 0 - 30: "))
#tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def body(dev, ctx):
global last_time
print "Doing something"
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = 30 #random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def tilt_and_sense():
global dev
while not dev:
time.sleep(1)
while 1:
time.sleep(3)
led = random.randint(0, 6)
tilt = random.randint(0, 30)
print('Led[%d] Tilt[%d]' % (led, tilt))
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print(freenect.raw_accel(dev))
print(freenect.mks_accel(dev))
def body(dev, ctx):
global last_time
if time.time() - last_time > 3 and not has_face:
last_time = time.time()
print 'NO FACE FOUND - Randomly tilting to see if I can find one..'
led = random.randint(0, 6)
freenect.set_led(dev, led)
tilt = random.randint(0, 30)
freenect.set_tilt_degs(dev, tilt)
if not keep_running:
raise freenect.Kill
def tilt_and_sense():
global dev
while not dev:
time.sleep(1)
while 1:
time.sleep(3)
led = random.randint(0, 6)
tilt = random.randint(0, 30)
print('Led[%d] Tilt[%d]' % (led, tilt))
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print(freenect.raw_accel(dev))
print(freenect.mks_accel(dev))
def body(dev, ctx):
global last_time
global led
global tilt
if not keep_running:
raise freenect.Kill
if led > 6 and tilt > 30:
return
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
time.sleep(1)
led = led + 1
tilt = tilt + 5
def body(*args):
# use globals in context
global calibrate_tilt_degs, tilt_degs
# print(freenect.get_accel(args[0]))
# if program has to calibrate tilt degs
if calibrate_tilt_degs:
# keep kinect on tilt_degs
print("current tilt degrees changed to "+str(tilt_degs))
freenect.set_tilt_degs(args[0],tilt_degs)
calibrate_tilt_degs = False
# stop runloop if keep_running is false
if not keep_running:
# shutdown kinect features
raise freenect.Kill
def mybody(*args):
dev, ctx = args
global last_time
print(last_time)
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def depth_view():
import matplotlib.pyplot as plt
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
x = np.arange(0, 256, 1)
zeros = np.zeros_like(x)
fig = plt.figure()
ax = fig.add_subplot(111)
view1, = ax.plot(x, zeros, '-k', label='black')
view2, = ax.plot(x, zeros, '-r', label='red')
np_array = np.array
np_max = np.max
view1_sety = view1.set_ydata
view2_sety = view2.set_ydata
ax_relim = ax.relim
ax_autoscale_view = ax.autoscale_view
while True:
dep = get_depth(False)
cv2.imshow('raw', dep)
dep = cv2.medianBlur(dep, 3)
bin = __get_bins(dep)
clean = copy(bin)
clean = __pipeline(clean)
bin[:2] = 0
clean *= np_max(bin)
view1_sety(bin)
view2_sety(clean)
ax_relim()
ax_autoscale_view()
im = fig2img(fig)
graph = np_array(im)
# dep = remove_background(dep, 100)
dep = isolate_depths(dep)
# dep = convert_to_bw(dep)
cv2.imshow('depth', dep)
cv2.imshow('graph', graph)
# show_image('all', frame, masked_frame, depth, video)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
def rand_led_tilt(dev, ctx):
"""Randomly changes the led and tilt position every 3 seconds"""
global last_time
if time.time() - last_time < 3:
return
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def body(dev, ctx):
global last_time
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
print "boop"
last_time = time.time()
led = random.randint(0, 6)
tilt = random.randint(0, 30)
print "set_led(%s)" % led
freenect.set_led(dev, led)
print "set_tilt_degs(%s)" % tilt
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def temp_test():
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
while True:
dep = get_depth()
dep *= (dep * 1.3).astype(np.uint8)
print("{}\t,\t{}".format(np.min(dep), np.max(dep)))
cv2.imshow('depth', dep)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
fn.sync_stop()
break
def body(dev):
global last_time
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
for i in range(0, 6):
led = i
tilt = i * 5
time.sleep(3)
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' %
(led, tilt, freenect.get_accel(dev)))
def set_tilt():
TILT_MAX = 30
TILT_STEP = 10
TILT_START = 0
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
if not dev:
freenect.error_open_device()
print "Starting TILT Cycle"
for tilt in xrange(TILT_START, TILT_MAX + TILT_STEP, TILT_STEP):
print "Setting TILT: ", tilt
freenect.set_led(dev, 6)
freenect.set_tilt_degs(dev, tilt)
time.sleep(3)
freenect.set_tilt_degs(dev, 0)
def body(dev, ctx):
global last_time
global tilt
global light
if not keep_running:
raise freenect.Kill
if tilt == 30:
return
if time.time() - last_time < 3:
return
last_time = time.time()
led = light
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def main(self, dev, ctx):
""" Instrucción ejecutada por el Thread.
Parametros:
dev --> Puntero del dispositivo de cámara identificado como Kinect.
ctx --> Puntero del Kinect.
Return: None
"""
if self.change_ang:
# Si la colección tiene más de un elemento:
if len(self.grados) > 1:
# Pide al Kinect moverse al ángulo ángulo y eliminarlo de la colección.
freenect.set_tilt_degs(dev, self.grados.pop())
else:
# Pide al Kinect moverse a la única posición de la colección.
freenect.set_tilt_degs(dev, self.grados[-1])
# Establece que el ángulo ya fué cambiado.
self.change_ang = False
def set_cam_angle(dev, new_angle):
if min_angle <= new_angle <= max_angle:
freenect.set_tilt_degs(dev, new_angle)
print("New Angle: {}".format(new_angle))
return new_angle
else:
if min_angle > new_angle:
print("New angle {} is lower than min angle {}".format(
new_angle, min_angle))
return min_angle
elif max_angle < new_angle:
print("New angle {} is higher than max angle {}".format(
new_angle, max_angle))
return max_angle
else:
raise ValueError("Wat? failed: {} <= {} <= {}".format(
min_angle, new_angle, max_angle))
def density_plot():
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
show_image = cv2.imshow
waitkey = cv2.waitKey
ravel = np.ravel
countbin = np.bincount
length = 256
nums = np.arange(0, length, 1)
zero = np.zeros_like(nums)
import matplotlib.pyplot as plt
import matplotlib.animation as animation
fig, ax = plt.subplots()
line, = ax.plot(nums, zero)
ax.set_ylim(0, 10000)
ax.set_xlim(0, 256)
set_y_data = line.set_ydata
def update(data):
set_y_data(data)
return line,
def get_dep():
dep = get_depth()
dep = cv2.medianBlur(dep, 3, dep)
dep = ravel(dep)
# dep = medfilt(dep, 21).astype(np.uint8)
return dep
def data_gen():
while True:
yield countbin(get_dep(), minlength=length)
ani = animation.FuncAnimation(fig, update, data_gen)
plt.show()
cv2.destroyAllWindows()
fn.sync_stop()
def loop(processimg):
ctx = freenect.init()
dev = freenect.open_device(ctx, 0)
freenect.set_tilt_degs(dev, 10)
freenect.close_device(dev)
while True:
#img = processimg(np.array(getimg_webcam()))
#gotimg, img = c.read()
img = getimg_irkinect()
#gotimg = True
#if not gotimg:
# print 'stopped'
# break
cv2.imshow('processing', processimg(img))
if cv2.waitKey(10) == 27:
break
cv2.destroyAllWindows()
def set_kinect_angle(angle, device_index=0):
# Clamp angle to [-30, 30]
angle = min(angle, max(angle, -30), 30)
print "Setting Kinect angle to", angle
# We have to stop the synchronous runloop to interact with the device.
freenect.sync_stop()
# Open the device
ctx = freenect.init()
dev = freenect.open_device(ctx, device_index)
# Set angle
freenect.set_tilt_degs(dev, angle)
# Shutdown context, allowing synchronous runloop to start
freenect.shutdown(ctx)
return angle
def body(dev, ctx):
"""
Execution body for the runloop, manages execution init for Kinect-related parameters.
Terminates all running processes to safely shutdown device services.
"""
global tilt_angle, state_file, tr_counter, file_manager
#Initializes tilt_angle variable for motor control
if tilt_angle == None:
freenect.update_tilt_state(dev)
tilt_angle = freenect.get_tilt_degs(freenect.get_tilt_state(dev))
#If the movement variable is updated, moves the motor
if not freenect.get_tilt_degs(freenect.get_tilt_state(dev)) == tilt_angle:
freenect.set_tilt_degs(dev, tilt_angle)
freenect.update_tilt_state(dev)
#Exit condition for finishing execution
if not keep_running:
del file_manager
cv.destroyAllWindows() # Kills all windows
raise freenect.Kill() # Shutdowns all Kinect's services
def body(dev, ctx):
global last_time, tilt0
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 1:
return
last_time = time.time()
led = 2
if faceFound and abs(tilt - tilt0) > 0:
led = 3
if faceFound and abs(tilt - tilt0) == 0:
led = 4
tilt0 = tilt
freenect.set_led(dev, led)
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
if not keep_running:
raise freenect.Kill
def body(dev,ctx):
"""
Execution body for the runloop, manages execution init for Kinect-related parameters.
Terminates all running processes to safely shutdown device services.
"""
global tilt_angle,state_file,tr_counter,file_manager
#Initializes tilt_angle variable for motor control
if tilt_angle==None:
freenect.update_tilt_state(dev)
tilt_angle=freenect.get_tilt_degs(freenect.get_tilt_state(dev))
#If the movement variable is updated, moves the motor
if not freenect.get_tilt_degs(freenect.get_tilt_state(dev))==tilt_angle:
freenect.set_tilt_degs(dev, tilt_angle)
freenect.update_tilt_state(dev)
#Exit condition for finishing execution
if not keep_running:
del file_manager
cv.destroyAllWindows() # Kills all windows
raise freenect.Kill() # Shutdowns all Kinect's services
def body(dev, ctx):
global last_time
if not keep_running:
raise freenect.Kill
if time.time() - last_time < 3:
return
last_time = time.time()
# rotate through the different colors
if led > 6:
led = 0
else:
led = led + 1
freenect.set_led(dev, led)
# tilt back and forth
if (tilt > 30 or tilt < 0):
tilt = 0
if (time.time() % 30 < 15):
tilt = tile + 5
else:
tilt = tilt - 5
freenect.set_tilt_degs(dev, tilt)
print('led[%d] tilt[%d] accel[%s]' % (led, tilt, freenect.get_accel(dev)))
def testkinect():
# test leds and tilts
[ctx, dev] = initkinect()
for i in range(1, 6):
freenect.set_led(dev, i)
cv2.waitKey(2000)
freenect.set_tilt_degs(dev, 0)
print('complete led')
cv2.waitKey(3000)
#test tilt
freenect.set_tilt_degs(dev, -50)
cv2.waitKey(3000)
freenect.set_tilt_degs(dev, 50)
cv2.waitKey(3000)
freenect.set_tilt_degs(dev, 0)
cv2.waitKey(1)
print('complete tilt')
freenect.shutdown(ctx)
* Function Name: return_mean
* Input: Depth Frame or any other matrix.
* Output: Returns mean of the frame
* Logic: It reduces the noise and calculates the mean of the pixel values in the frame using mean() function
* Example Call: return_mean(a)
"""
median = cv2.medianBlur(a,5)
mediane = cv2.medianBlur(a,5)
rect = mediane[0:479, 0:639]
mean = rect.mean()
return mean
ctx = freenect.init() #initialize freenect
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1) #open Kinect Device
freenect.set_tilt_degs(dev, 30) #Tilts kinect to 30 degrees
time.sleep(1)
freenect.set_tilt_degs(dev, 0) #Tilts kinect to 0 degree
time.sleep(1)
freenect.close_device(dev) #closes Kinect
while(True):
a = get_depth() #gets the depth data from Kinect
mean = return_mean(a) #returns the mean of the depth data
if mean > 240:
ser.write("\x38") #if the front area has more depth than the threshold than the robot will move forward
else:
while(return_mean(get_depth())<242):
ser.write("\x36") #rotate till the threshold is crossed
th3 = cv2.adaptiveThreshold(a,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,cv2.THRESH_BINARY,3,2) #Binary threshold
import freenect import sys ctx = freenect.init() dev = freenect.open_device(ctx, 0) freenect.set_tilt_degs(dev, float(sys.argv[1]))
def main(dev, ctx):
freenect.set_tilt_degs(dev, tilt)
def main():
rospy.init_node("camera")
rospy.loginfo("Starting Camera Node")
rospy.on_shutdown(shutdown_hook)
# Set initial camera angle
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
freenect.set_tilt_degs(dev, 20)
freenect.close_device(dev)
r = rospy.Rate(PUBLISH_RATE)
cv_bridge = CvBridge()
video_pub = rospy.Publisher('/camera/video', Image, queue_size=10)
depth_pub = rospy.Publisher('/camera/depth', Image, queue_size=10)
# Uses a different encoding for april tags
camera_image_pub = rospy.Publisher('/camera/image_raw',
Image,
queue_size=10)
camera_info_pub = rospy.Publisher('/camera/camera_info',
CameraInfo,
queue_size=10)
# Kinect manually calibrated using this http://people.csail.mit.edu/peterkty/teaching/Lab4Handout_Fall_2016.pdf
camera_info = CameraInfo()
hd = Header()
camera_info.height = 480
camera_info.width = 640
camera_info.distortion_model = "plumb_bob"
camera_info.D = [
0.16966890693679473, -0.32564392755677646, 0.0014273722857157428,
-0.0007780067287402459, 0.0
]
camera_info.K = [
522.7790149706918, 0.0, 317.5941836796907, 0.0, 523.5195539902463,
255.18973237498545, 0.0, 0.0, 1.0
]
camera_info.P = [
532.5130615234375, 0.0, 317.01325625466416, 0.0, 0.0, 535.176025390625,
255.7121671461573, 0.0, 0.0, 0.0, 1.0, 0.0
]
camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
while not rospy.is_shutdown():
video = get_video()
depth = get_depth()
video_msg = cv_bridge.cv2_to_imgmsg(video, encoding="passthrough")
depth_msg = cv_bridge.cv2_to_imgmsg(depth, encoding="passthrough")
cam_img_msg = cv_bridge.cv2_to_imgmsg(video, encoding="bgr8")
camera_info.header = video_msg.header
video_pub.publish(video_msg)
depth_pub.publish(depth_msg)
camera_image_pub.publish(cam_img_msg)
camera_info_pub.publish(camera_info)
r.sleep()
def main_vision(load):
# inits
fn_ctx = fn.init()
fn_dev = fn.open_device(fn_ctx, fn.num_devices(fn_ctx) - 1)
fn.set_tilt_degs(fn_dev, 0)
fn.close_device(fn_dev)
key_point = KeyPoints(150)
predictor = prediction(ModelPath)
preds = []
# optimization
t0 = 0.0
t1 = 0.0
fps = 0.0
total_fps = 0.0
frames = 0
kp_speed = key_point._get_kp_speedup()
draw_speed = key_point._get_draw_speedup()
proc_speed = key_point._get_process_speedup()
cvtColor = cv2.cvtColor
BGR2RGB = cv2.COLOR_BGR2RGB
get_kp = key_point.get_key_points
draw_kp = key_point.draw_key_points
process_image = key_point.__process_image
show_image = cv2.imshow
wait_for_key = cv2.waitKey
copy_thing = copy.copy
num_features = key_point.get_num_features()
arr_shape = np.shape
shape_check = (num_features, 32)
ravel = np.ravel
append_pred = preds.append
get_time = time.time
current_class = 0
if load:
brain = predictor.load_brain()
pred_speed = predictor.get_pred_speed()
predict = predictor.predict
else:
add_speed = predictor.get_add_speed()
add_data = predictor.add_data
get_length = predictor.get_data_length
if load:
net = Neural_Net(predictor.brain.getPoint(), np.vstack(predictor.brain.getData()), 4800 * 2, num_features)
nl_predict = net.predict
nl_speed = net.get_neural_speed()
# mainLoop
while True:
t0 = get_time()
# Get a fresh frame
depth = get_depth()
frame = get_video()
show_image('Raw Image', cvtColor(frame, BGR2RGB))
# Process Depth Image
# depth = remove_background(depth, 25)
depth = remove_background_percent(depth, .5, 50)
depth = convert_to_bw(depth)
mask = make_mask(depth)
# Process Image
frame = cvtColor(frame, BGR2RGB)
video = copy_thing(frame)
frame = process_image(frame, proc_speed)
# Make Masked Frame
masked_frame = copy_thing(frame)
masked_frame[mask] = 0
# Process Key Points
kp, des = get_kp(masked_frame, kp_speed)
video = draw_kp(video, kp, True, speedup=draw_speed)
# Predict current
if (load) and (des is not None) and (arr_shape(des) == shape_check):
pred = predict(ravel(des), pred_speed)
append_pred(pred)
print(pred)
print(nl_predict([ravel(des)], nl_speed))
# Add object description to data set
if (not load) and (des is not None) and (arr_shape(des) == shape_check):
add_data(add_speed, np.ravel(des), current_class)
print('Current Class and Length:\t%i\t%i' % (get_length(), current_class))
t1 = get_time()
fps = (1 / (t1 - t0))
total_fps += fps
frames += 1
print('%.2f FPS' % fps)
show_image('masked image', masked_frame)
show_image('depth', depth)
show_image('key points', video)
# show_image('all', frame, masked_frame, depth, video)
if wait_for_key(1) & 0xFF == ord('q'):
cv2.destroyAllWindows()
if load:
break
print('Current Class: %i\nn : Next Class\nr : Continue Current Class\nq : Quit' % (current_class))
inp = raw_input()
if inp == 'n':
current_class += 1
elif inp == 'q':
break
# print(np.mean(preds))
cv2.destroyAllWindows()
print('Average FPS: %.2f' % (total_fps / frames))
fn.sync_stop()
if not load:
predictor.create_brain()
main_vision(True)
def body(dev, ctx):
global KEEP_RUNNING
if not KEEP_RUNNING:
freenect.set_tilt_degs(dev, 0)
raise freenect.Kill
def body(self, dev, ctx):
if self.killfreenect:
raise freenect.Kill("Killing freenect runloop")
if self.tilt != self.last_tilt:
freenect.set_tilt_degs(dev, self.tilt)
self.last_tilt = self.tilt
def set_tilt(self, tilt):
if not self.dev:
print "no device set!"
return
freenect.set_tilt_degs(self.dev, tilt)
#!/usr/bin/env python
import freenect
import time
import sys
TILT_MAX = 30
TILT_STEP = 10
TILT_START = 0
if sys.argv[1]: TILT_MAX = int(sys.argv[1])
if sys.argv[2]: TILT_STEP = int(sys.argv[2])
if sys.argv[2]: TILT_START = int(sys.argv[3])
ctx = freenect.init()
dev = freenect.open_device(ctx, freenect.num_devices(ctx) - 1)
if not dev:
freenect.error_open_device()
print "Starting TILT Cycle"
for tilt in xrange(TILT_START, TILT_MAX+TILT_STEP, TILT_STEP):
print "Setting TILT: ", tilt
freenect.set_tilt_degs(dev, tilt)
time.sleep(3)
freenect.set_tilt_degs(dev, 0)
def set_angle(self, angle):
self.angle = angle
freenect.set_tilt_degs(self.dev, self.angle)
if sys.argv[1]: TILT_MAX = int(sys.argv[1])
if sys.argv[2]: TILT_STEP = int(sys.argv[2])
if sys.argv[2]: TILT_START = int(sys.argv[3])
ctx = freenect.init()
print "Number of kinects: %d\n" % freenect.num_devices(ctx)
dev = []
for devIdx in range(0, freenect.num_devices(ctx)):
print "opening device %d" % devIdx
dev.append(freenect.open_device(ctx, devIdx))
if not dev:
freenect.error_open_device()
print "Starting TILT Cycle"
for tilt in xrange(TILT_START, TILT_MAX+TILT_STEP, TILT_STEP):
for sensor in dev:
print "Setting TILT: ", tilt
freenect.set_tilt_degs(sensor, tilt)
time.sleep(1)
print "Starting TILT Cycle"
for tilt in xrange(TILT_MAX+TILT_STEP, TILT_START, (TILT_STEP * -1)):
for sensor in dev:
print "Setting TILT: ", tilt
freenect.set_tilt_degs(sensor, tilt)
time.sleep(1)
for sensor in dev:
print "Resetting TILT"
freenect.set_tilt_degs(sensor, -35)
