ros_vector_image_publisher.py

import rospy
import cv2
import numpy
import anki_vector

from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

if __name__ == "__main__":
    rospy.init_node('vector_publisher')


    image_pub = rospy.Publisher("vector_camera",Image, queue_size=1)
    cv_bridge = CvBridge()

    args = anki_vector.util.parse_command_args()


    with anki_vector.Robot(args.serial) as robot:
        robot.camera.init_camera_feed()

        last_id = 0
        try:

            while True:
                image = robot.camera.latest_image
                if image.image_id > last_id:

                    print("Got image")
                    last_id = image.image_id

                    cv_image = numpy.array(image.raw_image)
                    image_pub.publish(cv_bridge.cv2_to_imgmsg(cv_image, "bgr8"))

                    #cv2.imshow("Camera", frame)
                    #key = cv2.waitKey(10)

                    # On the other end...
                    #cv_image = cv_image.resize((184,96)) # Vector screen!
                    #im_pil = Image.fromarray(cv_image)
                    #screen_data = anki_vector.screen.convert_image_to_screen_data(im_pil)
                    #robot.screen.set_screen_with_image_data(screen_data, 0.04)

        except KeyboardInterrupt:
            print("Shutting down")