def main(): port = '/dev/ttyUSB0' bot = bot = pycreate2.Create2(port) bot.start() bot.safe() bot.drive_stop() image_size = (320, 240) camera = Camera(cam='pi') camera.init(win=image_size) imu = IMU() bag = BagWriter() bag.open(['camera', 'accel', 'mag', 'cr']) bag.stringify('camera') bag.use_compression = True try: for i in range(100): ret, frame = camera.read() if ret: bag.push('camera', frame) else: print('>> bad frame', i) sen = bot.get_sensors() bag.push('cr', sen) a, m, g = imu.get() bag.push('accel', a) bag.push('mag', m) # time.sleep(0.025) print(i) except KeyboardInterrupt: print(' ctrl-c pressed, bye ...') except Exception as e: print('') print('Something else happened ... :(') print('Maybe the robot is not on ... press the start button') print('-'*30) print(e) bag.write('test.json')
# calibration matrix if args['numpy'] is not None: cam_cal = args['numpy'] d = read(cam_cal) m = d['camera_matrix'] k = d['dist_coeff'] print('Using supplied camera calibration matrix: {}'.format(cam_cal)) # print size # print cam_cal save = SaveVideo() # open camera cap = Camera() cap.init(win=size, cameraNumber=source) print('---------------------------------') print(' ESC/q to quit') print(' v to start/stop video capture') print(' f to grab a frame') print('---------------------------------') shot_idx = 0 video_idx = 0 video = False vfn = None FPS = 30 sleep_time = 1.0/float(FPS) # 30 FPS # Main loop ---------------------------------------------
#!/usr/bin/env python from __future__ import print_function import cv2 from opencvutils import Camera cnt = 0 cam = Camera() # cam.gray = True cam.init(win=(320, 240), cameraNumber=0) while True: ret, im = cam.read() if ret: cv2.imshow('image', im) key = cv2.waitKey(1) & 0xFF if key == ord("q"): break elif key == ord("s"): cv2.imwrite("image-{}.png".format(cnt), im) print('>> Wrote: {}'.format("image-{}.png".format(cnt))) cnt += 1
def setUpCameraCV(win=(320, 240), cv=0): global camera camera = Camera('cv') camera.init(cameraNumber=cv, win=win)
def setUpCameraPi(win=(320, 240)): global camera camera = Camera('pi') camera.init(win=win)
#!/usr/bin/env python import cv2 import os from opencvutils import Camera import sys cascPath = 'haarcascade_frontalface_default.xml' faceCascade = cv2.CascadeClassifier(cascPath) image_size = (392, 280) camera = Camera(cam='pi') camera.init(win=image_size) while True: ret, img = camera.read() img = cv2.flip(img, 0) ##img = 255*img gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) faces = faceCascade.detectMultiScale( gray, scaleFactor=1.1, minNeighbors=5, minSize=(30, 30), ) for (x, y, w, h) in faces:
class Create2(object): bot = None imu = None camera = None sensors = {} def __init__(self, port='/dev/ttyUSB0'): self.bot = pycreate2.Create2(port) # only create if on linux: image_size = (640, 480) if platform.system() == 'Linux': self.camera = Camera(cam='pi') self.camera.init(win=image_size) self.imu = IMU() else: self.camera = Camera(cam='cv') self.camera.init(win=image_size, cameraNumber=0) self.sensors = {'create': None, 'imu': None, 'camera': None} self.distance = 0.0 self.ahrs = AHRS() self.modes = { 'js': JoyStickMode(self.bot), 'demo': DemoMode(self.bot), 'auto': AutoMode(self.bot), 'idle': IdleMode(self.bot), 'sensors': Sensors(self.bot) } # self.bag = Bag('./data/robot-{}.json'.format(time.ctime().replace(' ', '-')), ['imu', 'camera', 'create']) self.current_mode = 'idle' self.last_time = time.time() self.data = { # 'r': CircularBuffer(30), # 'p': CircularBuffer(30), 'y': CircularBuffer(30), } def __del__(self): # self.bag.close() self.bot.drive_direct(0, 0) self.current_mode = 'idle' print('Create2 robot is exiting ...') def setMode(self, mode): if mode in self.modes: self.current_mode = mode else: raise Exception( 'Create2::setMode selected mode is invalid: {}'.format(mode)) def run(self): self.bot.start() self.bot.safe() # self.bot.full() # self.setMode('js') self.setMode('demo') # self.setMode('idle') # self.setMode('auto') # self.setMode('sensors') while True: self.get_sensors() self.printInfo() # control roobma self.modes[self.current_mode].go(self.sensors) # time.sleep(0.05) def processImage(self, img): pass def printInfo(self): sensors = self.sensors['create'] imu = self.sensors['imu'] if sensors is None or imu is None: print('No valid sensor info') return a, m, g = imu now = time.time() dt = self.last_time - now beta = 0.5 q = self.ahrs.updateAGM(a, m, g, beta, dt) r, p, y = quat2euler(q) # self.data['r'].push(r) # self.data['p'].push(p) self.data['y'].push(y) self.last_time = now ir = [] for i in [36, 37, 38, 39, 40, 41]: ir.append(sensors[i]) cliff = [] for i in [20, 21, 22, 23]: cliff.append(sensors[i]) po = [ '--------------------------------------------------------', ' Light Bumper: {:6} {:6} {:6} L| {:6} |R {:6} {:6} {:6}'.format( sensors.light_bumper_left, sensors.light_bumper_front_left, sensors.light_bumper_center_left, sparkline.sparkify(ir).encode('utf-8'), # '', sensors.light_bumper_center_right, sensors.light_bumper_front_right, sensors.light_bumper_right), ' Cliff: {:6} {:6} L| {:4} |R {:6} {:6}'.format( sensors.cliff_left_signal, sensors.cliff_front_left_signal, sparkline.sparkify(cliff).encode('utf-8'), # '', sensors.cliff_front_right_signal, sensors.cliff_right_signal), ' Encoders: {:7} L|R {:7}'.format(sensors.encoder_counts_left, sensors.encoder_counts_right), ' Distance Delta: {:8} mm Total: {:10.1f} m'.format( sensors.distance, self.distance), # ' Yaw: {:8.1f} {:30} degrees'.format(self.data['y'].get_last(), self.data['y'].spark()), '--------------------------------------------------------', ' Power: {:6} mAhr [{:3} %]'.format( sensors.battery_charge, int(100.0 * sensors.battery_charge / sensors.battery_capacity)), ' Voltage: {:7.1f} V Current: {:7.1f} A'.format( sensors.voltage / 1000, sensors.current / 1000) ] for s in po: print(s) def get_sensors(self): cr = self.bot.get_sensors() self.sensors['create'] = cr # self.bag.push('create', cr) imu = self.imu.get() self.sensors['imu'] = imu # self.bag.push('imu', imu) self.distance += cr.distance / 1000.0 # if 'camera' in self.keys: self.sensors['camera'] = None ret, img = self.camera.read() if ret: self.sensors['camera'] = img else: print("*** No camera image ***")
class Create2(object): cr = None adc = None imu = None camera = None sensors = {} def __init__(self, port='/dev/ttyUSB0'): self.bot = pycreate2.Create2(port) # self.js = Joystick() self.camera = Camera(cam='cv') self.camera.init(win=(320, 240), cameraNumber=0) # only create if on linux, because: # imu needs access to i2c # adc needs access to spi # if platform.system() == 'Linux': # self.imu = IMU() # self.adc = MCP3208() print('Start') print(self.bot.inputCommands([35])) def __del__(self): # if self.bot: # self.bot.stop() pass def doPath(self): path = [['forward', 200, 2, 'for'], ['back', -200, 2, 'back'], ['stop', 0, 0.1, 'stop'], ['turn right', 100, 2, 'rite'], ['turn left', -100, 4, 'left'], ['turn right', 100, 2, 'rite'], ['stop', 0, 0.1, 'stop']] # path to move for mov in path: # move robot name, vel, dt, string = mov print('movement:', name) self.bot.digit_led_ascii(string) if name in ['forward', 'back', 'stop']: self.bot.drive_straight(vel) elif name in ['turn right', 'turn left']: self.bot.drive_turn(vel, -1) else: raise Exception('invalid movement command') # time.sleep(0.05) time.sleep(dt) def processSafety(self, ir, wheel_drops, bumpers): # print('ir', ir) level = 200 vel = 200 forward = (vel, 0) left = (vel, 1) right = (vel, -1) dt = 0 direction = forward reverse = False # handle wheel drops for danger in wheel_drops: if danger: print('crap ... lost the floor!!!') return (-vel, 0), 0 # handle physical bumpers if bumpers[0]: print('bumper left: hit something on our left') self.bot.drive_straight(-vel) time.sleep(0.5) return right, 2 if bumpers[1]: print('bumper right: hit something on our right') self.bot.drive_straight(-vel) time.sleep(0.5) return left, 2 # handle light bumpers if ir[0] > level: dt = 0.5 direction = right reverse = True if ir[1] > level: dt = 1.0 direction = right reverse = True if ir[2] > level: dt = 1.5 direction = right reverse = True if ir[5] > level: dt = 0.5 direction = left reverse = True if ir[4] > level: dt = 1.0 direction = left reverse = True if ir[3] > level: dt = 1.5 direction = left reverse = True if reverse: self.bot.drive_straight(-vel) time.sleep(0.5) return direction, dt def run(self): print('run') print(self.bot.inputCommands([35])) self.bot.start() print('after start') print(self.bot.inputCommands([35])) self.bot.safe() print('after safe') print(self.bot.inputCommands([35])) # self.bot.full() exit(0) # health = [14, 22, 23, 24, 25, 26, 54, 55] # nav = [19, 20, 107] # safety = [7, 9, 10, 11, 12, 46, 47, 48, 49, 50, 51] # path = [ # ['forward', 200, 2, 'for'], # ['back', -200, 2, 'back'], # ['stop', 0, 0.1, 'stop'], # ['turn right', 100, 2, 'rite'], # ['turn left', -100, 4, 'left'], # ['turn right', 100, 2, 'rite'], # ['stop', 0, 0.1, 'stop'] # ] light_bumper = [ 'light bump left signal', 'light bump front left signal', 'light bump center left signal', 'light bump center right signal', 'light bump front right signal', 'light bump right signal', ] curr_dist = 0.0 # rn for a certain number of steps for _ in range(200): # get sensor readings # health_info = self.bot.inputCommands(health) # nav_info = self.bot.inputCommands(nav) # safety_info = self.bot.inputCommands(safety) # print('sensors:', info) safety_info = self.bot.inputCommands([100]) # get everything if not safety_info: exit(1) ir = [] for key in light_bumper: ir.append(safety_info[key]) tmp = safety_info['wheel drop and bumps'] # print('wheel/dump', tmp) # wheel_drops = ((tmp & (1<<3)) > 0, (tmp & (1<<2)) > 0) # left, right # bumpers = ((tmp & (1<<1)) > 0, (tmp & (1)) > 0) # left, right wheel_drops = (tmp['drop left'], tmp['drop right']) bumpers = (tmp['bump left'], tmp['bump right']) direction, dt = self.processSafety(ir, wheel_drops, bumpers) vel = direction[0] if dt != 0: self.bot.drive_turn(vel, direction[1]) time.sleep(dt) else: self.bot.drive_straight(vel) time.sleep(0.1) sensor_data = safety_info print('=' * 40) print('stasis', sensor_data['stasis']) curr_dist += sensor_data['distance'] print('distance', curr_dist) print('-' * 40) print('left motor current [mA]', sensor_data['left motor current']) print('right motor current [mA]', sensor_data['right motor current']) print( 'battery status [%]:', 100 * sensor_data['battery charge'] / sensor_data['battery capacity']) print('voltage:', sensor_data['voltage']) print('current:', sensor_data['current']) # grab camera image if self.camera: ok, img = self.camera.read() if ok: # good image capture self.processImage(img) else: print('<<< bad image >>>') # time.sleep(0.5) def command(self, vel, rot, scale=1.0): """ vel: 0-1 rot: 0-2pi scale: scalar multiplied to velocity """ vel *= scale if rot < 0.02: # a little more than a degree self.bot.drive_straight(vel) else: i = int(rot * 10) i = 0 if i < 0 else i i = 10 if i > 10 else i r = range(1100, 0, -100) radius = r[i] self.bot.drive_turn(vel, radius) print('cmd:', vel, radius) def processImage(self, img): # find ar code # print('image size', img.shape) pass