# rospy for the subscriber
import rospy
# ROS Image message
from sensor_msgs.msg import Image
# ROS Image message -> OpenCV2 image converter
from cv_bridge import CvBridge, CvBridgeError
# OpenCV2 for saving an image
import cv2
# save numpy array as npy file
import numpy as np
from numpy import asarray
from numpy import save

kernel3 = np.ones((3,3),np.uint8)

# Instantiate CvBridge
bridge = CvBridge()

def image_callback(msg):
    print("Received an image!")
    try:
        # Convert your ROS Image message to OpenCV2
        cv2_img = bridge.imgmsg_to_cv2(msg, "passthrough")
    except CvBridgeError, e:
        print(e)
    else:
        save('/home/smartinezs/iri-lab/labrobotica/python/Grasp-demo/sergi/data.npy', cv2_img) 

def image_rgb_callback(msg):
    print("Received an rgb image!")
    try:
        # Convert your ROS Image message to OpenCV2
        cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
    except CvBridgeError, e:
        print(e)
    else:
        cv2.imwrite('/home/smartinezs/iri-lab/labrobotica/python/Grasp-demo/sergi/camera_image.jpeg', cv2_img)
 
        
def main():
    rospy.init_node('image_listener')
    # Define your image topic
    image_topic = "/depth/image_raw"
    image_rgb_topic = "/rgb/image_raw"
    # Set up your subscriber and define its callback
    rospy.Subscriber(image_topic, Image, image_callback)
    rospy.Subscriber(image_rgb_topic, Image, image_rgb_callback)
    # Spin until ctrl + c
    rospy.spin()

if __name__ == '__main__':
    main()