0
$\begingroup$

Rosanswers logo

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 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
target_FPS = 20

# 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 = target_FPS
# one of these camera options causes instability, hangs:
#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')

# init messages
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 = 'stereo_camera' # TF frame

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 = 'stereo_camera'

print("Setup done, entering main loop")
framecount=0
frametimer=time.clock()
# 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:, :]

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

        stamp = rospy.Time.now()

        left_img_msg.header.stamp = stamp
        left_img_msg.data = left_image.flatten().tolist()

        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)
        
        if time.clock() > frametimer +1:
            print("approx FPS: ", framecount, "target FPS: ", target_FPS)
            frametimer=time.clock()
            framecount=0
                   
        


except rospy.ROSInterruptException:
    camera.close()
    pass

This works reliably but it's really slow. The publishing framerate is 10 when it should be 20. It's only using about 66% of one core. When i subscribe using rqt_image_view on my laptop, the rate drops to 5 FPS. What's going on?

Originally posted by Wezzoid on ROS Answers with karma: 76 on 2018-06-02

Post score: 0

Original comments

Comment by gvdhoorn on 2018-06-02:
Is it just the publishing that stops, or the rest as well (ie: if you remove all publishing related code, does it still stop after ~500 iterations)?

Comment by gvdhoorn on 2018-06-02:
Also:

right_img_msg.header.frame_id = 'image_raw'

this makes no sense to me: frame_id is supposed to contain a ref to a TF frame. image_raw is typically a topic name.

Additionally: why recreate the msg each time instead of just updating the contents (ie: data, stamp)?

Comment by Wezzoid on 2018-06-02:
Thanks gvdhoorn! You're right it's happening earlier than the publisher. One of the camera options must have caused it. I don't know which one and I'm not going to investigate further right now, I need to push on and do the calibration and camera_info publishing! I adjusted the code (updated OP)

Comment by Wezzoid on 2018-06-02:
It now works reliably but it's really slow. The publishing framerate is 10 when it should be 20. It's only using about 66% of one core. When i subscribe using rqt_image_view on my laptop, the rate drops to 5 FPS. What's going on there?

Comment by Wezzoid on 2018-06-03:
fixed the speed issue. will put an answer in

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

Doing antishake, video stabilisation and AWB all at once makes it unstable for some reason. As for the speed issues, it's reading the stream into a 3d array then laboriously flattening it again, which is pointless unless I need to do OpenCV stuff with the image in this node. Bypassing this speeds it up. This works quite well, though it doesn't have the camera_info incorporated yet:

#!/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 rospy
from sensor_msgs.msg import CameraInfo, Image
import yaml
import io
import signal # for ctrl-C handling
import sys


# cam resolution
res_x = 320 #320 # per camera
res_y = 240 #240 
target_FPS = 15

# initialize the camera
print "Init camera..."
camera = PiCamera(stereo_mode = 'top-bottom',stereo_decimate=False)
camera.resolution = (res_x, res_y*2) # top-bottom stereo
camera.framerate = target_FPS
# using several camera options can cause instability, hangs after a while
camera.exposure_mode = 'antishake'
#camera.video_stabilization = True # fussy about res?

stream = io.BytesIO()
 
# ----------------------------------------------------------
#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=1)
right_img_pub = rospy.Publisher('right_camera/image_color', Image, queue_size=1)
rospy.init_node('stereo_pub')

# init messages
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 = 'stereo_camera' # TF frame

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 = 'stereo_camera'

imageBytes = res_x*res_y*3

# ---------------------------------------------------------------
# this is supposed to shut down gracefully on CTRL-C but doesn't quite work:
def signal_handler(signal, frame):
    print 'CTRL-C caught'
    print 'closing camera'
    camera.close()
    time.sleep(1)
    print 'camera closed'    
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)

#-----------------------------------------------------------

print "Setup done, entering main loop"
framecount=0
frametimer=time.time()
toggle = True
# capture frames from the camera
for frame in camera.capture_continuous(stream, format="rgb", use_video_port=True):
    framecount +=1
    
    stamp = rospy.Time.now()
    left_img_msg.header.stamp = stamp
    right_img_msg.header.stamp = stamp
    
    frameBytes = stream.getvalue()    
    left_img_msg.data = frameBytes[:imageBytes]
    right_img_msg.data = frameBytes[imageBytes:]      

    #publish the image pair
    left_img_pub.publish(left_img_msg)
    right_img_pub.publish(right_img_msg)
    
    # console info
    if time.time() > frametimer +1.0:
        if toggle: 
            indicator = '  o' # just so it's obviously alive if values aren't changing
        else:
            indicator = '  -'
        toggle = not toggle        
        print 'approx publish rate:', framecount, 'target FPS:', target_FPS, indicator
        frametimer=time.time()
        framecount=0
        
    # clear the stream ready for next frame
    stream.truncate(0)
    stream.seek(0)

Originally posted by Wezzoid with karma: 76 on 2018-06-03

This answer was ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.