def readImage_thread(self, child_conn): count = 0 time0 = time.time() time1 = time.time() data = {} while self.running: if ArducamSDK.Py_ArduCam_availableImage(self.handle) > 0: rtn_val, data, rtn_cfg = ArducamSDK.Py_ArduCam_readImage( self.handle) datasize = rtn_cfg['u32Size'] if rtn_val != 0 or datasize == 0: self.ArducamSDK.Py_ArduCam_del(self.handle) print("read data fail!") continue image_data = self.dBytesToMat(data, self.BitWidth, self.Width, self.Height) child_conn.send(image_data) time1 = time.time() if time1 - time0 >= 1: # print("%s %d %s\n"%("camera fps:",count,"/s")) count = 0 time0 = time1 count += 1 ArducamSDK.Py_ArduCam_del(self.handle) # time.sleep(.01) else: time.sleep(0.005) child_conn.close()
def captureImage_thread(self): rtn_val = ArducamSDK.Py_ArduCam_beginCaptureImage(self.handle) if rtn_val != 0: print("Error beginning capture, rtn_val = ", rtn_val) self.running = False return else: print("Capture began, rtn_val = ", rtn_val) while self.running: if self.loading: rtn_val = ArducamSDK.Py_ArduCam_captureImage(self.handle) if rtn_val > 255: # print("Error capture image, rtn_val = ",rtn_val) if rtn_val == ArducamSDK.USB_CAMERA_USB_TASK_ERROR: break else: print("restarting camera...") # time.sleep(1) ArducamSDK.Py_ArduCam_endCaptureImage(self.handle) self.camera_initFromFile(self.fileName, self.expconf) ArducamSDK.Py_ArduCam_beginCaptureImage(self.handle) self.loading = True self.running = False ArducamSDK.Py_ArduCam_endCaptureImage(self.handle)
def readThread(threadName,read_Flag): global flag,handle count = 0 time0 = time.time() time1 = time.time() data = {} cv2.namedWindow("MT9M001",1) cv2.setMouseCallback("MT9M001",mouse_callback) while flag: if ArducamSDK.Py_ArduCam_available(handle) > 0: res,data = ArducamSDK.Py_ArduCam_read(handle,Width * Height) if res == 0: count += 1 time1 = time.time() ArducamSDK.Py_ArduCam_del(handle) else: print "read data fail!" else: print "is not available" if len(data) >= Width * Height: if time1 - time0 >= 1: print "%s %d %s\n"%("fps:",count,"/s") count = 0 time0 = time1 show(data) else: print "data length is not enough!" if flag == False: break
def video(): global flag, regArr, handle regNum = 0 res, handle = ArducamSDK.Py_ArduCam_autoopen(cfg) if res == 0: openFlag = True print "device open success!" while (regArr[regNum][0] != 0xFFFF): ArducamSDK.Py_ArduCam_writeSensorReg(handle, regArr[regNum][0], regArr[regNum][1]) regNum = regNum + 1 res = ArducamSDK.Py_ArduCam_beginCapture(handle) if res == 0: print "transfer task create success!" while flag: res = ArducamSDK.Py_ArduCam_capture(handle) if res != 0: print "capture fail!" break time.sleep(0.03) if flag == False: break else: print "transfer task create fail!" res = ArducamSDK.Py_ArduCam_close(handle) if res == 0: openFlag = False print "device close success!" else: print "device close fail!" else: print "device open fail!"
def __init__(self, blink_control=None, serialtouse=None, config_file_name="AR0135_1280x964_ext_trigger_M.json"): if not os.path.exists(config_file_name): print("Config file does not exist.") exit() devices_num, index, serials = ArducamSDK.Py_ArduCam_scan() print("Found %d devices" % devices_num) self.indextouse = None for i in range(devices_num): datas = serials[i] serial = "" for it, d in enumerate(datas[0:12]): serial = serial + "%c" % d if (it % 4) == 3 and it < 11: serial = serial + "-" if serialtouse is None: if i == 0: usethis = True else: if serial == serialtouse: usethis = True if usethis: self.indextouse = i print("Index:", index[i], "Serial:", serial, "Use:", usethis) time.sleep(2) self.handle = self.camera_initFromFile(config_file_name) if self.handle != None: ret_val = ArducamSDK.Py_ArduCam_setMode( self.handle, ArducamSDK.EXTERNAL_TRIGGER_MODE) if (ret_val == ArducamSDK.USB_BOARD_FW_VERSION_NOT_SUPPORT_ERROR): print("USB_BOARD_FW_VERSION_NOT_SUPPORT_ERROR") exit(0) self.prs = QB() self.blink_control = blink_control
def process_thread(self): global handle, running, Width, Height, save_flag, cfg, color_mode global COLOR_BayerGB2BGR, COLOR_BayerRG2BGR, COLOR_BayerGR2BGR, COLOR_BayerBG2BGR while running: if ArducamSDK.Py_ArduCam_availableImage(handle) > 0: self.rawImageTimeStamp = datetime.datetime.now() self.imageTimeStamp = datetime.datetime.now().strftime( '%Y_%m_%d_%H_%M_%S') self.imageName = 'imagem_%s.jpg' % self.imageTimeStamp self.imagePath = "images/" + self.imageName rtn_val, data, rtn_cfg = ArducamSDK.Py_ArduCam_readImage( handle) datasize = rtn_cfg['u32Size'] if rtn_val != 0: print("ardu cam read data fail!!") if datasize == 0: print("ardu cam read data size = 0!!") image = convert_image(data, rtn_cfg, color_mode) if save_flag: cv2.imwrite(self.imagePath, image) self.image = np.uint8( ndimage.imread(self.imagePath, flatten=True)) self.image = self.crop_end(self.image, 0, 150) save_flag = False ArducamSDK.Py_ArduCam_del(handle) else: time.sleep(0.01)
def captureImage(): global handle,Width,Height,cfg,color_mode global COLOR_BayerGB2BGR,COLOR_BayerRG2BGR,COLOR_BayerGR2BGR,COLOR_BayerBG2BGR rtn_val = ArducamSDK.Py_ArduCam_captureImage(handle) if rtn_val > 255: print "Error capture image, rtn_val = ",rtn_val if rtn_val == ArducamSDK.USB_CAMERA_USB_TASK_ERROR: exit() if ArducamSDK.Py_ArduCam_availableImage(handle) > 0: rtn_val,data,rtn_cfg = ArducamSDK.Py_ArduCam_readImage(handle) datasize = rtn_cfg['u32Size'] if rtn_val != 0 or datasize == 0: print "read data fail!" ArducamSDK.Py_ArduCam_del(handle) return image = convert_image(data,rtn_cfg,color_mode) if h_flip: image = cv2.flip(image, 1) if v_flip: image = cv2.flip(image, 0) try: img_msg = bridge.cv2_to_imgmsg(image, "bgr8") img_msg.header.stamp = rospy.Time.now() img_msg.header.frame_id = id_frame pub.publish(img_msg) cv2.waitKey(10) except CvBridgeError as e: pass ArducamSDK.Py_ArduCam_del(handle) else: time.sleep(0.001)
def captureImage_thread(): global handle, running, ct_lock, count1 rtn_val = ArducamSDK.Py_ArduCam_beginCaptureImage(handle) if rtn_val != 0: print "Error beginning capture, rtn_val = ", rtn_val running = False return else: print "Capture began, rtn_val = ", rtn_val while (running and (not rospy.is_shutdown())): #print "capture" if count1 == 0: if ct_lock.acquire(False): print "capture image", count1 rtn_val = ArducamSDK.Py_ArduCam_captureImage(handle) count1 = count1 + 1 if rtn_val != 0: print "Error capture image, rtn_val = ", rtn_val break else: time.sleep(0.005) ct_lock.release() else: time.sleep(0.005) else: time.sleep(0.005) running = False ArducamSDK.Py_ArduCam_endCaptureImage(handle)
def capture(request): global handle,Width,Height,cfg,color_mode global COLOR_BayerGB2BGR,COLOR_BayerRG2BGR,COLOR_BayerGR2BGR,COLOR_BayerBG2BGR if ArducamSDK.Py_ArduCam_availableImage(handle) > 0: rtn_val,data,rtn_cfg = ArducamSDK.Py_ArduCam_readImage(handle) datasize = rtn_cfg['u32Size'] if rtn_val != 0: print "read data fail!" return CaptureResponse("Failed to Capture") if datasize == 0: return CaptureResponse("Failed to Capture") image = convert_image(data,rtn_cfg,color_mode) if h_flip: image = cv2.flip(image, 1) if v_flip: image = cv2.flip(image, 0) try: img_msg = bridge.cv2_to_imgmsg(image, "bgr8") img_msg.header.stamp = rospy.Time.now() img_msg.header.frame_id = id_frame pub_capture.publish(img_msg) return CaptureResponse("Captured") except CvBridgeError as e: return CaptureResponse("Failed to Capture") else: return CaptureResponse("Failed to Capture")
def cameramain(self, config_file_name, expconf, child_conn): if self.camera_initFromFile(config_file_name, expconf): ArducamSDK.Py_ArduCam_setMode(self.handle, ArducamSDK.CONTINUOUS_MODE) ct = threading.Thread(target=self.captureImage_thread) rt = threading.Thread(target=self.readImage_thread, args=(child_conn, )) ct.start() rt.start() while True: reload = child_conn.recv() print("reloading configuration file") if reload == False: self.loading = False if reload == 'quit': self.running = False sys.exit() break ct.join() rt.join() rtn_val = ArducamSDK.Py_ArduCam_close(self.handle) if rtn_val == 0: print("device close success!") else: print("device close fail!")
def rosShutdown(): global handle ArducamSDK.Py_ArduCam_endCaptureImage(handle) rtn_val = ArducamSDK.Py_ArduCam_close(handle) if rtn_val == 0: print "device close success!" else: print "device close fail!"
def stop(self): self.logger.info("Stopping arducam") self.running = False ArducamSDK.Py_ArduCam_del(self.handle) ArducamSDK.Py_ArduCam_flush(self.handle) ArducamSDK.Py_ArduCam_endCaptureImage(self.handle) ArducamSDK.Py_ArduCam_close(self.handle) self.images.clear() self.logger.info("Arducam stopped")
def writeSingleSensorReg(reg_str,val_str): global handle regAddr = int(reg_str,16) val = int(val_str,16) ArducamSDK.Py_ArduCam_writeSensorReg(handle,regAddr,val) #check if value sucessfully changed: if (val == ArducamSDK.Py_ArduCam_readSensorReg(handle,regAddr)[1]): print ("SUCCESFULLY CHANGED" + ' ' + hex(regAddr) + ' ' + "TO" + ' ' + hex(val)) else: rospy.logerr ("WRITING COMMAND NOT SUCCESFULL")
def camera_initFromFile(fialeName): global cfg,handle,Width,Height,color_mode #load config file config = json.load(open(fialeName,"r")) camera_parameter = config["camera_parameter"] Width = int(camera_parameter["SIZE"][0]) Height = int(camera_parameter["SIZE"][1]) BitWidth = camera_parameter["BIT_WIDTH"] ByteLength = 1 if BitWidth > 8 and BitWidth <= 16: ByteLength = 2 FmtMode = int(camera_parameter["FORMAT"][0]) color_mode = (int)(camera_parameter["FORMAT"][1]) rospy.loginfo("color mode %d",color_mode) I2CMode = camera_parameter["I2C_MODE"] I2cAddr = int(camera_parameter["I2C_ADDR"],16) TransLvl = int(camera_parameter["TRANS_LVL"]) cfg = {"u32CameraType":0x4D091031, "u32Width":Width,"u32Height":Height, "usbType":0, "u8PixelBytes":ByteLength, "u16Vid":0, "u32Size":0, "u8PixelBits":BitWidth, "u32I2cAddr":I2cAddr, "emI2cMode":I2CMode, "emImageFmtMode":FmtMode, "u32TransLvl":TransLvl } ret,handle,rtn_cfg = ArducamSDK.Py_ArduCam_autoopen(cfg) if ret == 0: usb_version = rtn_cfg['usbType'] rospy.loginfo("USB VERSION:%d",usb_version) #config board param configBoard(config["board_parameter_dev2"]) writeSensorRegs(config["register_parameter"]) rtn_val,datas = ArducamSDK.Py_ArduCam_readUserData(handle,0x400-16, 16) rospy.loginfo("Serial: %c%c%c%c-%c%c%c%c-%c%c%c%c",datas[0],datas[1],datas[2],datas[3], datas[4],datas[5],datas[6],datas[7], datas[8],datas[9],datas[10],datas[11]) return True else: rospy.logfatal("Device open fail (rtn_val = %d)",ret) return False
def readThread(threadName, read_Flag): global flag, handle, storeFlag, bufferData, openFlag global a_lock count = 0 time0 = time.time() time1 = time.time() data = {} # Wait for the arducam object to be ready while openFlag == False: time1 = time.time() if time1 - time0 > 20: #timeout exit while flag: res = ArducamSDK.Py_ArduCam_available(handle) #~ print "Available frames %d"%(res) if res > 0: res, data = ArducamSDK.Py_ArduCam_read(handle, Width * Height) if res == 0: count += 1 time1 = time.time() ArducamSDK.Py_ArduCam_del(handle) else: print("read data fail!") else: #print "No data availiable" time.sleep(.01) if len(data) >= Width * Height: if time1 - time0 >= 5: print("%s %f %s\n" % ("fps:", count * 1.0 / (time1 - time0), "/s")) count = 0 time0 = time1 a_lock.acquire() bufferData = data data = [] storeFlag = True a_lock.release() #show(data) #else: # print "data length is not enough!" if flag == False: break
def read_next_frame(self): code = ArducamSDK.Py_ArduCam_captureImage(self.handle) if code > 255: raise ArducamException("Error capturing image", code=code) if ArducamSDK.Py_ArduCam_availableImage(self.handle): try: rtn_val, data, rtn_cfg = ArducamSDK.Py_ArduCam_readImage( self.handle) if rtn_val != 0 or rtn_cfg['u32Size'] == 0: raise ArducamException( "Bad image read! Datasize was {}".format( rtn_cfg['u32Size']), code=rtn_val) return convert_image(data, rtn_cfg, self.color_mode) finally: ArducamSDK.Py_ArduCam_del(self.handle)
def start_capture(self): self.logger.info("Start arducam capture thread") start_code = ArducamSDK.Py_ArduCam_beginCaptureImage(self.handle) if start_code != 0: raise ArducamException("Error starting capture thread", code=start_code) self.logger.debug("Thread started")
def getSingleFrame(self, handle): #global running,Width,Height,save_flag,cfg,color_mode,totalFrames,save_raw count = 0 print("Take picture.") rtn_val, data, rtn_cfg = ArducamSDK.Py_ArduCam_getSingleFrame(handle) if rtn_val != 0: print("Take picture fail,ret_val = ", rtn_val) return datasize = rtn_cfg['u32Size'] if datasize == 0: print("data length zero!") return print(datasize) print(type(data)) im = np.frombuffer(data, np.uint8, count=datasize).reshape(self.height, self.width) if self.blink_control is not None: direction = self.blink_control.direction flash = self.blink_control.flashselection else: direction = None flash = None self.prs.put(PhotoResult(im, direction, flash))
def capture_thread(self): global handle, running rtn_val = ArducamSDK.Py_ArduCam_beginCaptureImage(handle) if rtn_val != 0: print("Error beginning capture, rtn_val = ", rtn_val) else: logging.info("Capture began, rtn_val = ", rtn_val) while running: rtn_val = ArducamSDK.Py_ArduCam_captureImage(handle) if rtn_val > 255: #print("Error capture image, rtn_val = ", rtn_val) if rtn_val == ArducamSDK.USB_CAMERA_USB_TASK_ERROR: print("ardu cam USB_CAMERA_USB_TASK_ERROR!!") time.sleep(0.01)
def writeSensorRegs(fileNodes): global handle for i in range(0, len(fileNodes)): fileNode = fileNodes[i] regAddr = int(fileNode[0], 16) val = int(fileNode[1], 16) ArducamSDK.Py_ArduCam_writeSensorReg(handle, regAddr, val)
def rosShutdown(): global handle rtn_val = ArducamSDK.Py_ArduCam_close(handle) if rtn_val == 0: rospy.loginfo("device close success!") else: rospy.logerr("device close fail!")
def readImage_thread(): global handle, running, Width, Height, save_flag, cfg, color_mode, save_raw global COLOR_BayerGB2BGR, COLOR_BayerRG2BGR, COLOR_BayerGR2BGR, COLOR_BayerBG2BGR count = 0 totalFrame = 0 time0 = time.time() time1 = time.time() data = {} cv2.namedWindow("ArduCam Demo", 1) if not os.path.exists("images"): os.makedirs("images") while running: display_time = time.time() if ArducamSDK.Py_ArduCam_availableImage(handle) > 0: rtn_val, data, rtn_cfg = ArducamSDK.Py_ArduCam_readImage(handle) datasize = rtn_cfg['u32Size'] if rtn_val != 0: print("read data fail!") continue if datasize == 0: continue image = convert_image(data, rtn_cfg, color_mode) time1 = time.time() if time1 - time0 >= 1: print("%s %d %s\n" % ("fps:", count, "/s")) count = 0 time0 = time1 count += 1 if save_flag: cv2.imwrite("images/image%d.jpg" % totalFrame, image) if save_raw: with open("images/image%d.raw" % totalFrame, 'wb') as f: f.write(data) totalFrame += 1 image = cv2.resize(image, (640, 480), interpolation=cv2.INTER_LINEAR) cv2.imshow("ArduCam Demo", image) cv2.waitKey(10) ArducamSDK.Py_ArduCam_del(handle) #print("------------------------display time:",(time.time() - display_time)) else: time.sleep(0.001)
def write_regs(self, reg_name): for r in self.get_register_value(reg_name): if r[0] == "DELAY": time.sleep(float(r[1]) / 1000) continue self.logger.debug("Writing register to cam {0}: {1}".format( self.dev_id, r)) ArducamSDK.Py_ArduCam_writeSensorReg(self.handle, int(r[0], 16), int(r[1], 16))
def writeSensorRegs(handle, fileNodes): for i in range(0, len(fileNodes)): fileNode = fileNodes[i] if fileNode[0] == "DELAY": time.sleep(float(fileNode[1]) / 1000) continue regAddr = int(fileNode[0], 16) val = int(fileNode[1], 16) ArducamSDK.Py_ArduCam_writeSensorReg(handle, regAddr, val)
def initCamera(self): try: if self.camera_initFromFile(self.config_file_name): ArducamSDK.Py_ArduCam_setMode(handle, ArducamSDK.CONTINUOUS_MODE) return self.OP_OK except Exception as ex: logging.exception("Error during camera initialization!") return self.OP_ERR
def read_register(request): global handle register = request.register rtn_val, output = ArducamSDK.Py_ArduCam_readSensorReg(handle,register) if rtn_val == 0: output = 'Register %d: %d' % (register, output) else: output = 'Invalid register' return ReadRegResponse(output)
def write_register(request): global handle register = request.register value = request.value rtn_val = ArducamSDK.Py_ArduCam_writeSensorReg(handle,register,value) if rtn_val == 0: output = 'Value %d written to register %d' % (value, register) else: output = 'Invalid register' return WriteRegResponse(output)
def get_extra_label_info(self): if self.data_fields['LUM2'] == 0: self.data_fields['LUM2'] = ArducamSDK.Py_ArduCam_readSensorReg( self.handle, int(12546))[1] if self.field_index == 0 or self.data_fields['TIME'] == 0: self.data_fields['TIME'] = ArducamSDK.Py_ArduCam_readSensorReg( self.handle, int(12644))[1] if self.field_index == 1 or self.data_fields['TIME'] == 0: self.data_fields['ISO'] = ArducamSDK.Py_ArduCam_readSensorReg( self.handle, int(12586))[1] if self.field_index == 2 or self.data_fields['TIME'] == 0: self.data_fields['LUM1'] = ArducamSDK.Py_ArduCam_readSensorReg( self.handle, int(12626))[1] self.field_index = self.field_index + 1 if self.field_index < 2 else 0 return [("Time: {0} ISO: {1} LUM:{2}/{3}").format( self.data_fields['TIME'], self.data_fields['ISO'], self.data_fields['LUM1'], self.data_fields['LUM2'])]
def show2(): global flag, regArr, handle global W_zoom, H_zoom, V_value, H_value, lx, ly, downFlag, saveFlag, saveNum regNum = 0 res, handle = ArducamSDK.Py_ArduCam_autoopen(cfg) if res == 0: openFlag = True print "device open success!" while (regArr[regNum][0] != 0xFFFF): ArducamSDK.Py_ArduCam_writeSensorReg(handle, regArr[regNum][0], regArr[regNum][1]) regNum = regNum + 1 res = ArducamSDK.Py_ArduCam_beginCapture(handle) if res == 0 and flag: print "transfer task create success!" res = ArducamSDK.Py_ArduCam_capture(handle) if ArducamSDK.Py_ArduCam_available(handle) > 0: print "estou available" res, data = ArducamSDK.Py_ArduCam_read(handle, Width * Height) image = Image.frombuffer("L", (Width, Height), data) img = np.array(image) height, width = img.shape[:2] img2 = cv2.cvtColor(img, COLOR_BYTE2RGB) if args.name and capture: cv2.imwrite(args.name + "." + args.type, img2) if not args.show: return if saveFlag: saveFlag = False saveNum += 1 name = "" if "bmp" == args.type: name = str(saveNum) + ".bmp" if "png" == args.type: name = str(saveNum) + ".png" if "jpg" == args.type: name = str(saveNum) + ".jpg" cv2.imwrite(name, img2) if res != 0: print "capture fail!" return if flag == False: return else: print "transfer task create fail!" res = ArducamSDK.Py_ArduCam_close(handle) if res == 0: openFlag = False print "device close success!" else: print "device close fail!" else: print "device open fail!"
def configBoard(handle,fileNodes): for i in range(0,len(fileNodes)): fileNode = fileNodes[i] buffs = [] command = fileNode[0] value = fileNode[1] index = fileNode[2] buffsize = fileNode[3] for j in range(0,len(fileNode[4])): buffs.append(int(fileNode[4][j],16)) ArducamSDK.Py_ArduCam_setboardConfig(handle,int(command,16),int(value,16),int(index,16),int(buffsize,16),buffs)