RGB+D image subscriber

#! /usr/bin/python


import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np
import sys
import time


# Instantiate CvBridge
bridge = CvBridge()
c = -1


def image_callback(msg):

    print("\nReceived an image!")
    try:
        # Convert your ROS Image message to OpenCV2
        if image_topic == "/kinect2/hd/image_color":
            cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")

        else:
            depth_img = bridge.imgmsg_to_cv2(msg, "32FC1")
            depth_img = np.array(depth_img, dtype=np.float32)
            cv2.normalize(depth_img, depth_img, 0, 1, cv2.NORM_MINMAX)

    except CvBridgeError, e:
        print(e)

    else:
        global c
        if(c == 24):
            c = 0
            print "Pictures taken - issue CTRL+C"
            time.sleep(2)
        c += 1

        if image_topic == "/kinect2/hd/image_color":
            # Save the OpenCV2 image as png
            cv2.imwrite('data/rgbd/lorenzos_desk/color_'+str(c)+'.png', cv2_img)
        else:
            np.save('data/rgbd/lorenzos_desk/depth_'+str(c)+'.npy', np.asarray(depth_img))


def main():
    global image_topic

    image_topic = str(sys.argv[1])
    rospy.init_node('image_listener')
    rospy.Subscriber(image_topic, Image, image_callback)
    # Spin until ctrl + c
    rospy.spin()

if __name__ == '__main__':
    main()