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.fps = FPS()
示例#2
0
def handle_pub(videoCapture):
    topic = '/file/video'
    dashes = '_' * 60
    print dashes
    print "publish video to topic:%s from file ..." % (topic)
    print dashes
    rospy.init_node('file_publisher')
    pub = rospy.Publisher(topic, Image, queue_size=2)

    bridge = CvBridge()
    fps = FPS()
    fps = fps.start()
    success, img = videoCapture.read()
    rate = rospy.Rate(PlayFPS)
    count = 0
    while success:
        img_copy = img.copy()
        try:
            msg = bridge.cv2_to_imgmsg(img, "bgr8")
            count += 1
            msg = add_timestamp(msg, count)

            if verbose:
                show_data_info(msg, fps)
        except CvBridgeError as e:
            print(e)

        pub.publish(msg)

        if show_video:
            try:
                show_image(img_copy, fps)
            except (ExitLoop, KeyboardInterrupt, SystemExit):
                cleanup(videoCapture)
                break

        success, img = videoCapture.read()
        rate.sleep()
示例#3
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()
     self.hosts = {}
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.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), 1)

        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)

    # ...

    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

        utility.show_msg_info(msg, showLatency=True)
        print ("fps: %s" % self.fps)

    def cleanup(self):
        print self.shutdowm_msg
        cv2.destroyAllWindows()
示例#5
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()
        self.hosts = {}

    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, FaceDetect, self.callback)

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

    def callback(self, msg):
        self.show_data_info(msg)
        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 count_frames(self, msg):
        host = msg.header.frame_id.split('-')[0]
        if host in self.host:
            self.host[host] += 1
        else:
            self.host[host] = 1

    def show_data_info(self, msg):
        import utility
        result = 'header: \n'
        result += str(msg.header) + '\n'
        result = utility.pinned_prefix(result)
        secs = int(msg.header.stamp.secs)
        nsecs = int(msg.header.stamp.nsecs)
        latency = rospy.Time.now() - rospy.Time(secs, nsecs)
        latency_ms = latency.to_nsec() / 1000000.0
        self.count_frames(msg)
        print result
        print 'latency: %.3f ms\n' % (latency_ms)

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