Guest User

Pi stereo cam ROS publisher

a guest
Oct 19th, 2018
719
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2.  
  3. # picamera stereo ROS node using dual CSI Pi CS3 board
  4. # Wes Freeman 2018
  5. # modified from code by Adrian Rosebrock, pyimagesearch.com
  6. # and jensenb, https://gist.github.com/jensenb/7303362
  7.  
  8. from picamera.array import PiRGBArray
  9. from picamera import PiCamera
  10. import time
  11. import rospy
  12. from sensor_msgs.msg import CameraInfo, Image
  13. import yaml
  14. import io
  15. import signal # for ctrl-C handling
  16. import sys
  17.  
  18.  
  19. def parse_calibration_yaml(calib_file):
  20.     with file(calib_file, 'r') as f:
  21.         params = yaml.load(f)
  22.  
  23.     cam_info = CameraInfo()
  24.     cam_info.height = params['image_height']
  25.     cam_info.width = params['image_width']
  26.     cam_info.distortion_model = params['distortion_model']
  27.     cam_info.K = params['camera_matrix']['data']
  28.     cam_info.D = params['distortion_coefficients']['data']
  29.     cam_info.R = params['rectification_matrix']['data']
  30.     cam_info.P = params['projection_matrix']['data']
  31.  
  32.     return cam_info
  33.  
  34.  
  35. # cam resolution
  36. res_x = 320 #320 # per camera
  37. res_y = 240 #240
  38. target_FPS = 15
  39.  
  40. # initialize the camera
  41. print "Init camera..."
  42. camera = PiCamera(stereo_mode = 'top-bottom',stereo_decimate=False)
  43. camera.resolution = (res_x, res_y*2) # top-bottom stereo
  44. camera.framerate = target_FPS
  45. # using several camera options can cause instability, hangs after a while
  46. camera.exposure_mode = 'antishake'
  47. #camera.video_stabilization = True # fussy about res?
  48.  
  49. stream = io.BytesIO()
  50.  
  51. # ----------------------------------------------------------
  52. #setup the publishers
  53. print "init publishers"
  54. # queue_size should be roughly equal to FPS or that causes lag?
  55. left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)
  56. right_img_pub = rospy.Publisher('right/image_raw', Image, queue_size=1)
  57.  
  58. left_cam_pub = rospy.Publisher('left/camera_info', CameraInfo, queue_size=1)
  59. right_cam_pub = rospy.Publisher('right/camera_info', CameraInfo, queue_size=1)
  60.  
  61. rospy.init_node('stereo_pub')
  62.  
  63. # init messages
  64. left_img_msg = Image()
  65. left_img_msg.height = res_y
  66. left_img_msg.width = res_x
  67. left_img_msg.step = res_x*3 # bytes per row: pixels * channels * bytes per channel (1 normally)
  68. left_img_msg.encoding = 'rgb8'
  69. left_img_msg.header.frame_id = 'stereo_camera' # TF frame
  70.  
  71. right_img_msg = Image()
  72. right_img_msg.height = res_y
  73. right_img_msg.width = res_x
  74. right_img_msg.step = res_x*3
  75. right_img_msg.encoding = 'rgb8'
  76. right_img_msg.header.frame_id = 'stereo_camera'
  77.  
  78. imageBytes = res_x*res_y*3
  79.  
  80. # parse the left and right camera calibration yaml files
  81. left_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/left.yaml')
  82. right_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/right.yaml')
  83.  
  84. # ---------------------------------------------------------------
  85. # this is supposed to shut down gracefully on CTRL-C but doesn't quite work:
  86. def signal_handler(signal, frame):
  87.     print 'CTRL-C caught'
  88.     print 'closing camera'
  89.     camera.close()
  90.     time.sleep(1)
  91.     print 'camera closed'    
  92.     sys.exit(0)
  93.  
  94. signal.signal(signal.SIGINT, signal_handler)
  95.  
  96. #-----------------------------------------------------------
  97.  
  98. print "Setup done, entering main loop"
  99. framecount=0
  100. frametimer=time.time()
  101. toggle = True
  102. # capture frames from the camera
  103. for frame in camera.capture_continuous(stream, format="rgb", use_video_port=True):
  104.     framecount +=1
  105.    
  106.     stamp = rospy.Time.now()
  107.     left_img_msg.header.stamp = stamp
  108.     right_img_msg.header.stamp = stamp
  109.     left_cam_info.header.stamp = stamp
  110.     right_cam_info.header.stamp = stamp    
  111.    
  112.     left_cam_pub.publish(left_cam_info)
  113.     right_cam_pub.publish(right_cam_info)    
  114.    
  115.     frameBytes = stream.getvalue()    
  116.     left_img_msg.data = frameBytes[:imageBytes]
  117.     right_img_msg.data = frameBytes[imageBytes:]      
  118.  
  119.     #publish the image pair
  120.     left_img_pub.publish(left_img_msg)
  121.     right_img_pub.publish(right_img_msg)
  122.    
  123.     # console info
  124.     if time.time() > frametimer +1.0:
  125.         if toggle:
  126.             indicator = '  o' # just so it's obviously alive if values aren't changing
  127.         else:
  128.             indicator = '  -'
  129.         toggle = not toggle        
  130.         print 'approx publish rate:', framecount, 'target FPS:', target_FPS, indicator
  131.         frametimer=time.time()
  132.         framecount=0
  133.        
  134.     # clear the stream ready for next frame
  135.     stream.truncate(0)
  136.     stream.seek(0)
RAW Paste Data