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)