import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

class ImageSubscriber(Node):
  def __init__(self):
    super().__init__('depthimage_subscriber')
    self.subscription = self.create_subscription(
      Image, 
      'depth_image', 
      self.listener_callback, 
      10)
    self.subscription # prevent unused variable warning
    self.br = CvBridge()

  def listener_callback(self, data):
    # Convert ROS Image message to OpenCV image
    current_frame = self.br.imgmsg_to_cv2(data) #np.ndarray
   	#minVal, maxVal, min_loc, max_loc = cv2.minMaxLoc(current_frame)
    clipmin = 0.3
    clipmax = 2.0
    
    new_frame = current_frame.copy()
    np.clip(new_frame, clipmin, clipmax, out=new_frame)
    new_frame -= clipmin
    new_frame /= (clipmax-clipmin)/255.0
    new_frame = new_frame.astype(dtype=np.uint8, copy=False)

    im_color = cv2.applyColorMap(new_frame, cv2.COLORMAP_JET); # cv2.COLORMAP_AUTUMN
  
    cv2.imshow("camera", im_color)
    cv2.waitKey(1) # 1 for automatic update

  
def main(args=None):
  
  rclpy.init(args=args)
  image_subscriber = ImageSubscriber()
  rclpy.spin(image_subscriber)
  image_subscriber.destroy_node()
  rclpy.shutdown()
  
if __name__ == '__main__':
  main()
