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)