예제 #1
0
class VideoPub:

    def __init__(self, capture, topic, show_window=False, show_info=False, rate=32):
        self.show_window = show_window
        self.show_info = show_info
        self.videoCapture = capture
        self.topic = topic
        self.rate = rate
        self.bridge = CvBridge()
        self.KEY_ECS = 27
        self.imgPlayer = ImagePlayer()
        self.mutex = threading.Lock()
        self.fps = FPS()
        self.msg = None

    def run(self):
        self.handle_pub()

    def handle_pub(self):

        rospy.init_node('video_publisher')
        pub = rospy.Publisher(topic, Image, queue_size=2)
        self.handle_threading()

        count = 0
        self.fps.start()
        rate = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            success, img = self.videoCapture.read()
            if not success:
                break

            try:
                msg = self.bridge.cv2_to_imgmsg(img, "bgr8")
            except CvBridgeError as e:
                print(e)
                rospy.signal_shutdown(e)

            count += 1
            self.fps.update()
            self.msg = self.add_timestamp(msg, count)
            pub.publish(self.msg)

            self.show_image(img)
            rate.sleep()
        # end wile
    # ....

    def handle_threading(self):
        threading.Thread(target=self.exit_key_event).start()
        if self.show_info:
            threading.Thread(target=self.show_data_info).start()

    def exit_key_event(self):
        while not rospy.is_shutdown():
            with self.mutex:
                ch = getch()
                if len(ch) > 0 and (ch == 'q' or ord(ch) == imtools.KEY_CTRL_C):
                    print 'user press crtl + c or q to exit'
                    rospy.signal_shutdown('user press crtl + c or q to exit')
                    break

    def add_timestamp(self, msg, count):
        now = rospy.Time.now()
        msg.header.frame_id = get_hostname() + '-' + str(count)
        msg.header.stamp.secs = now.secs
        msg.header.stamp.nsecs = now.nsecs
        return msg

    def show_image(self, img):
        if self.show_window:
            self.imgPlayer.show(img)
            key = self.imgPlayer.get_key()
            if 0xFF & key == self.KEY_ECS:
                rospy.signal_shutdown('user press ESC to exit')

    def show_data_info(self):
        import utility
        import time
        while not rospy.is_shutdown():
            time.sleep(.5)
            with self.mutex:
                utility.show_msg_info(self.msg)
                print('fps: %s' % round(self.fps, 2))
                print '%s' % (quit_msg)

    def cleanup(self):
        self.videoCapture.release()
        cv2.destroyAllWindows()
예제 #2
0
class ImgSub:

    def __init__(self, topic, should_mirror, verbose):
        self.topic = topic
        self.KEY_ECS = 27
        self.should_mirror = should_mirror
        self.verbose = verbose
        self.bridge = CvBridge()
        rospy.on_shutdown(self.cleanup)
        self.shutdowm_msg = "Shutting down."
        self.node_name = 'image_subscriber'
        self.time_start = clock()
        self.frames = []
        self.frame_max = 90
        self.imgPlayer = ImagePlayer("show %s" % (self.topic))

    def run(self):
        print '%s node turn on' % (self.node_name)
        self.handle_sub()
        self.show_video()
        try:
            rospy.spin()
        except KeyboardInterrupt:
            self.cleanup()

    def handle_sub(self):
        rospy.init_node(self.node_name, anonymous=True)
        self.sub = rospy.Subscriber(self.topic, Image, self.callback)

    def callback(self, msg):

        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            if self.verbose:
                self.show_data_info(msg)
        except CvBridgeError as e:
            print(e)

        cv_image = self.mirror_image(cv_image)
        if len(self.frames) >= self.frame_max:
            self.frames.pop(0)

        self.frames.append(cv_image)
    # ...

    def show_video(self):
        while not rospy.is_shutdown():
            if len(self.frames) > 0:
                img = self.frames.pop(0)
                self.imgPlayer.show(img)

                key = self.imgPlayer.get_key()
                if key == self.KEY_ECS:
                    rospy.signal_shutdown("User hit q key to quit.")
                elif key == ord('a'):
                    file_name = 'image_%s.jpg' % (str(int(clock())))
                    cv2.imwrite(file_name, img)
                    print '%s has saved.' % file_name

    def mirror_image(self, img):
        if self.should_mirror:
            from imtools import mirror_image
            img = mirror_image(img)
        return img

    def show_data_info(self, msg):
        import utility
        utility.show_msg_info(msg, showLatency=True)
        print 'fps: %s' % (self.imgPlayer.get_fps())

    def cleanup(self):
        print self.shutdowm_msg
        cv2.destroyAllWindows()