Esempio n. 1
0
 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
Esempio n. 2
0
 def __init__(self, topic, should_mirror, verbose, limit=1000):
     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.limit = limit
     self.frame_count = 0
     self.total_latency = 0
     self.fps = FPS()
Esempio n. 3
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()
Esempio n. 4
0
class ImgSub:

    def __init__(self, topic, should_mirror, verbose, limit=1000):
        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.limit = limit
        self.frame_count = 0
        self.total_latency = 0
        self.fps = FPS()

    def run(self):
        print '%s node turn on' % (self.node_name)
        self.fps = self.fps.start()
        self.handle_sub()

    def handle_sub(self):
        rospy.init_node(self.node_name, anonymous=True)
        cv2.namedWindow("show %s" % (self.topic), 10)

        self.sub = rospy.Subscriber(self.topic, Image, self.callback)

        try:
            rospy.spin()
        except KeyboardInterrupt:
            print(self.shutdowm_msg)
            cv2.destroyAllWindows()

    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)
        self.show_video(cv_image)
        self.frame_count += 1
        if self.frame_count >= self.limit:
            rospy.signal_shutdown("reach the limit of frames.")
    # ...

    def show_video(self, img):
        self.fps.update()
        draw_str(img, (5, 30), 'fps: %s' % self.fps)

        cv2.imshow("show %s" % (self.topic), img)
        key = cv2.waitKey(1)
        if 0xFF & key == self.KEY_ECS:
            rospy.signal_shutdown("User hit q key to quit.")
        elif 0xFF & 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
        self.total_latency += utility.show_msg_info(msg, showLatency=True)
        print('fps: %s' % self.fps)

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