#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

bridge = CvBridge()
import cv2
i = 0

def converter(msg):
    global i
    image = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    cv2.namedWindow("output", cv2.WINDOW_NORMAL)
    cv2.imshow("unique_window_identifier", image)
    cv2.setWindowTitle("unique_window_identifier", f"robot_camera_{i}.jpg")
    k = cv2.waitKey(1) 
    if k & 0xFF == ord(' '):
        cv2.imwrite("robot_camera_{}.jpg".format(i), image)
        rospy.loginfo("Image num: {}".format(i))
        i += 1
    if k == ord('q'):
        cv2.destroyAllWindows()
        rospy.signal_shutdown()
    
def main():
    rospy.init_node("node_to_jpg")
    rospy.Subscriber("/turtlebot_1/camera/rgb/image_raw", Image, queue_size=1, callback=converter)
    rospy.spin()

if __name__ == "__main__":
    main()
