custom stereo camera publisher fails mysteriously

edit

I'm trying to make a stereo camera for visual odometry. My Python script to publish the images consistently stops after ~500 images, with no errors.

I have cameras plugged into the 2 CSI ports on the standard Raspberry Pi Compute Module breakout board. I'm using Kinetic on Raspian. I'm using the picamera library so I can access the cameras as if I was using raspivid. This brings in both cameras as one image with good sync (better than 1/60 sec anyway) which i then split up and publish using sensor_msgs. I think sensor_msgs is the only way to do it as I need to set identical timestamps, but I'm a novice.

Also the publish rate is pretty slow. When I ask for 20 FPS I get about 5. message queue is set to 10. Would that be a problem? I suspect I'm going to have to redo it in C++ but I'd rather avoid that because python is quick to use.

Can someone tell me what I'm doing wrong, or a better way?

#!/usr/bin/env python

# picamera stereo ROS node using dual CSI Pi CS3 board
# Wes Freeman 2018
# modified from code by Adrian Rosebrock, pyimagesearch.com
# and jensenb, https://gist.github.com/jensenb/7303362

from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import numpy as np
import cv2
import rospy
from sensor_msgs.msg import CameraInfo, Image
import yaml

# cam resolution
# x must be multiple of 128: 640, 512, 384
res_x = 384 # per camera
res_y = 288

# initialize the camera
print("Init camera...")
camera = PiCamera(stereo_mode = 'side-by-side',stereo_decimate=False)
camera.resolution = (res_x*2, res_y)
camera.framerate = 5
#camera.exposure_mode = 'sports'
camera.exposure_mode = 'antishake'
camera.video_stabilization = True
camera.awb_mode = 'tungsten'

rawCapture = PiRGBArray(camera, size=(res_x*2, res_y)) # picamera struct, a numpy array?

#setup the publishers
print("init publishers")
# queue_size should be roughly equal to FPS?
left_img_pub = rospy.Publisher('left_camera/image_color', Image, queue_size=10)
right_img_pub = rospy.Publisher('right_camera/image_color', Image, queue_size=10)
rospy.init_node('stereo_pub')

print("Setup done, entering main loop")
framecount=0
# capture frames from the camera
try:
    for frame in camera.capture_continuous(rawCapture, format="rgb", use_video_port=True):
        framecount +=1
        # grab the raw NumPy array representing the image and split it
        left_image = frame.array[:, :res_x, :]
        right_image = frame.array[:, res_x+1:, :]

        # clear the stream in preparation for the next frame
        rawCapture.truncate(0)

        stamp = rospy.Time.now()

        left_img_msg = Image()
        left_img_msg.height = res_y
        left_img_msg.width = res_x
        left_img_msg.step = res_x*3 # bytes per row: pixels * channels * bytes per channel (1 normally)
        left_img_msg.encoding = 'rgb8'
        left_img_msg.header.frame_id = 'image_raw'
        left_img_msg.header.stamp = stamp
        left_img_msg.data = left_image.flatten().tolist()

        right_img_msg = Image()
        right_img_msg.height = res_y
        right_img_msg.width = res_x
        right_img_msg.step = res_x*3
        right_img_msg.encoding = 'rgb8'
        right_img_msg.header.frame_id = 'image_raw'
        right_img_msg.header.stamp = stamp
        right_img_msg.data = right_image.flatten().tolist()

        #publish the image pair
        left_img_pub.publish(left_img_msg)
        right_img_pub.publish(right_img_msg)

        print("published frame: ", framecount)           



except rospy.ROSInterruptException:
    camera.close()
    pass

Revised code after advice in this thread:

#!/usr/bin/env python

# picamera stereo ROS node using dual CSI ...