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 "Found %d devices" % devices_num for i in range(devices_num): datas = serials[i] 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]) print "Index:", index[i], "Serial:", serial time.sleep(2) for i in range(devices_num): handle = camera_initFromFile(config_file_name, i) if handle != None: ret_val = ArducamSDK.Py_ArduCam_setMode(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) handles.append(handle) totalFrames.append(0) while running and len(handles): for i in range(len(handles)): handle = handles[i] value = ArducamSDK.Py_ArduCam_isFrameReady(handle) if value == 1: getAndDisplaySingleFrame(handle, i) # else: # ArducamSDK.Py_ArduCam_softTrigger(handle)#soft trigger cv2.waitKey(10)
def worker(self): while True: ArducamSDK.Py_ArduCam_softTrigger(self.handle) if ArducamSDK.Py_ArduCam_isFrameReady(self.handle): self.getSingleFrame(self.handle)