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)