#!/usr/bin/env python

""" cv_bridge_demo.py - Version 0.1 2011-05-29

    A ROS-to-OpenCV node that uses cv_bridge to map a ROS image topic and optionally a ROS
    depth image topic to the equivalent OpenCV image stream(s).
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2011 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""

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

class cvBridgeDemo():
    def __init__(self):
        self.node_name = "cv_bridge_demo"
        
        rospy.init_node(self.node_name)
        
        # Porządkowanie podczas zamykania systemu
        rospy.on_shutdown(self.cleanup)
        
        # Utworzenie okna OpenCV przeznaczonego na obraz RGB
        self.cv_window_name = self.node_name
        cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
        cv.MoveWindow(self.cv_window_name, 25, 75)
        
        # Kolejne okno dla obrazu głębi
        cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
        cv.MoveWindow("Depth Image", 25, 350)
        
        # Utworzenie obiektu cv_bridge
        self.bridge = CvBridge()
        
        # Subskrypcja obrazu kamery, motywów głębi i ustawienie
        # odpowiednich wywołań zwrotnych
        self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback)
        self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
        
        rospy.loginfo("Waiting for image topics...")

    def image_callback(self, ros_image):
        # Wykorzystanie cv_bridge() do konwersji obrazu ROS na format OpenCV
        try:
            frame = self.bridge.imgmsg_to_cv(ros_image, "bgr8")
        except CvBridgeError, e:
            print e
        
        # Konwersja obrazu na tablicę Numpy. Większość funkcji cv2
        # wymaga tablic Numpy.
        frame = np.array(frame, dtype=np.uint8)
        
        # Przetwarzanie klatki za pomocą funkcji process_image()
        display_image = self.process_image(frame)
                       
        # Wyświetlenie obrazu
        cv2.imshow(self.node_name, display_image)
        
        # Przetwarzanie poleceń z klawiatury
        self.keystroke = cv.WaitKey(5)
        if 32 <= self.keystroke and self.keystroke < 128:
            cc = chr(self.keystroke).lower()
            if cc == 'q':
                # Użytkownik nacisnął klawisz q — zakończenie pracy
                rospy.signal_shutdown("Użytkownik nacisnął q, aby zakończyć pracę.")
                
    def depth_callback(self, ros_image):
        # Wykorzystaj metodę cv_bridge() w celu konwersji obrazu ROS na format OpenCV
        try:
            # Obraz głębi jest jednokanałowym obrazem float32
            depth_image = self.bridge.imgmsg_to_cv(ros_image, "32FC1")
        except CvBridgeError, e:
            print e

        # Konwersja obrazu głębi na tablicę Numpy. Większość funkcji cv2
        # wymaga tablic Numpy
        depth_array = np.array(depth_image, dtype=np.float32)
                
        ## Normalizacja obrazu głębi w przedziale pomiędzy 0 (czerń), a 1 (biel)
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
        
        # Przetwarzanie obrazu głębi
        depth_display_image = self.process_depth_image(depth_array)
    
        # Wyświetlenie wyników
        cv2.imshow("Depth Image", depth_display_image)
          
    def process_image(self, frame):
        # Konwersja na skalę szarości
        grey = cv2.cvtColor(frame, cv.CV_BGR2GRAY)
        
        # Rozmazanie obrazu
        grey = cv2.blur(grey, (7, 7))
        
        # Przeliczenie krawędzi za pomocą filtra krawędzi Canny
        edges = cv2.Canny(grey, 15.0, 30.0)
        
        return edges
    
    def process_depth_image(self, frame):
        # na potrzeby tego demo zwróć surowy obraz
        return frame
    
    def cleanup(self):
        print "Zamykanie węzła wizji."
        cv2.destroyAllWindows()   
    
def main(args):       
    try:
        cvBridgeDemo()
        rospy.spin()
    except KeyboardInterrupt:
        print "Zamykanie węzła wizji."
        cv.DestroyAllWindows()

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