image_tk.py

#!/usr/bin/env python
import roslib
roslib.load_manifest('rgbd_people_tracking')
import sys
import os
import rospy
import cv2
import matplotlib.pyplot as plt
from std_msgs.msg import String
from sensor_msgs.msg import Image
from sensor_msgs.msg import CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import Tkinter as tk

np.set_printoptions(threshold=np.nan)

root = tk.Tk()

w = tk.Canvas(root, width=500, height=300, bd = 10, bg = 'white')
w.grid(row = 0, column = 0, columnspan = 2)

b = tk.Button(width = 10, height = 2, text = 'Button1')
b.grid(row = 1, column = 0)

class CameraTracking:

  def __init__(self):
    self.bridge = CvBridge()
    self.rgb_sub = rospy.Subscriber("/burt/kinect2/hd/image_color_rect",Image,self.rgb_callback)
    self.depth_sub = rospy.Subscriber("/burt/kinect2/hd/image_depth_rect",Image,self.depth_callback)
    self.camera_info_sub = rospy.Subscriber("/burt/kinect2/hd/camera_info",Image,self.camera_info_callback)
    self.camera_info = None #Initialises empty, will contain camera metadata, including Intrinsic Parameters (see docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)
    self.camera_extrinsic_parameters = None #Camera Extrinsic Parameters will Load from YAML file

  def rgb_callback(self,data):
    try:
      rgb_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)
    cv2.namedWindow("RGB",1)
    w.create_image(0,0, image=rgb_image)
    if cv2.WaitKey(10) == 27:
      print("button press!")

  def depth_callback(self,data):
      try:
        # The depth image is a single-channel float32 image
        # the values is the distance in mm in z axis
        depth_image = self.bridge.imgmsg_to_cv2(data, '32FC1')
        
        '''
        # Having issues getting depth image to display using CV2 window, invalid format
        #Attempt of converting

        depth_array = np.array(depth_image, dtype=np.float32)
        # Normalize the depth image to fall between 0 (black) and 1 (white)
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
        # At this point you can display the result properly:
        # cv2.imshow('Depth Image', depth_display_image)

        # If you write it as it si, the result will be a image with only 0 to 1 values.
        
        cv2.namedWindow("Depth")
        cv2.imshow("Depth", depth_image)
        cv2.waitKey(1)
        '''
      except CvBridgeError, e:
        print e

  def camera_info_callback(self,data):
    self.camera_info = data

def main(args):
  CameraTracking()
  rospy.init_node('image_converter', anonymous=True)

  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)