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 getAndDisplaySingleFrame(handle, index):
global running, Width, Height, save_flag, cfg, color_mode, totalFrames, save_raw
global COLOR_BayerGB2BGR, COLOR_BayerRG2BGR, COLOR_BayerGR2BGR, COLOR_BayerBG2BGR
count = 0
totalFrame = totalFrames[index]
time0 = time.time()
time1 = time.time()
# data = {}
windowName = "ArduCam%d" % index
cv2.namedWindow(windowName, 1)
save_path = "images%d" % index
if not os.path.exists(save_path):
os.makedirs(save_path)
print "Take picture."
display_time = time.time()
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
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("%s/image%d.jpg" % (save_path, totalFrame), image)
print "Camera%d,save image%d" % (index, totalFrame)
if save_raw:
with open("%s/image%d.raw" % (save_path, totalFrame), 'wb') as f:
f.write(data)
totalFrames[index] += 1
image = cv2.resize(image, (640, 480), interpolation=cv2.INTER_LINEAR)
cv2.imshow(windowName, image)
cv2.waitKey(10)
print "End display."
def getAndDisplaySingleFrame(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)
self.ascii_draw_image(im[0::10, 0::5])
def time_subscriber_callback(time_data):
global next_trigger_counter, first_trigger
if first_trigger:
next_trigger_counter = time_data.header.seq
first_trigger = False
if next_trigger_counter == time_data.header.seq:
while ArducamSDK.Py_ArduCam_isFrameReady(handle) != 1:
continue
while True:
rtn_val,data,rtn_cfg = ArducamSDK.Py_ArduCam_getSingleFrame(handle)
if rtn_val == 0:
break
datasize = rtn_cfg['u32Size']
if datasize > 0:
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:
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
img_msg = bridge.cv2_to_imgmsg(image, "mono8")
exposure_time = rospy.Time.from_sec(0.01) #adding exposure time 20ms/2
img_msg.header.stamp.secs = time_data.header.stamp.secs + exposure_time.to_sec()
img_msg.header.stamp.nsecs = time_data.header.stamp.nsecs + exposure_time.to_nsec()
img_msg.header.seq = time_data.header.seq
img_msg.header.frame_id = id_frame
pub_trigger.publish(img_msg)
except CvBridgeError as e:
rospy.logwarn("Frame not published")
else:
rospy.logwarn("trigger not in sync (seq expected %u, got %u)!", next_trigger_counter, time_data.header.seq)
next_trigger_counter += 1
print("USB_BOARD_FW_VERSION_NOT_SUPPORT_ERROR")
exit()
service_write = rospy.Service('arducam/write_reg', WriteReg,
write_register)
service_read = rospy.Service('arducam/read_reg', ReadReg,
read_register)
service_trigger = rospy.Service('arducam/trigger', Trigger, trigger)
captured = 0
soft_trigger = False
rospy.on_shutdown(rosShutdown)
while not rospy.is_shutdown():
value = ArducamSDK.Py_ArduCam_isFrameReady(handle)
if value == 1:
rtn_val, data, rtn_cfg = ArducamSDK.Py_ArduCam_getSingleFrame(
handle)
if rtn_val != 0:
rospy.sleep(0.001)
if soft_trigger:
captured = 1
soft_trigger = False
continue
datasize = rtn_cfg['u32Size']
if datasize == 0:
rospy.sleep(0.001)
if soft_trigger:
captured = 1
soft_trigger = False
continue
image = convert_image(data, rtn_cfg, color_mode)
if h_flip: