<commit_before>#!/usr/bin/env python

from PyQt4 import QtGui, QtCore, QtNetwork
import rospkg
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image, CameraInfo
import rospy
import cv2
import numpy as np
import qimage2ndarray
from std_msgs.msg import Bool
from std_srvs.srv import Empty, EmptyResponse
import message_filters
from image_geometry import PinholeCameraModel
from geometry_msgs.msg import PointStamped, Pose, PoseArray
import tf
import ast

# TODO create ProjectorROS (to separate QT / ROS stuff)
# podle vysky v pointcloudu / pozice projektoru se vymaskuji mista kde je neco vyssiho - aby se promitalo jen na plochu stolu ????


class Projector(QtGui.QWidget):

    def __init__(self):

        super(Projector, self).__init__()

        self.proj_id = rospy.get_param('~projector_id', 'test')
        self.world_frame = rospy.get_param('~world_frame', 'marker')
        self.screen = rospy.get_param('~screen_number', 0)
        self.camera_image_topic = rospy.get_param('~camera_image_topic', '/kinect2/hd/image_color_rect')
        self.camera_depth_topic = rospy.get_param('~camera_depth_topic', '/kinect2/hd/image_depth_rect')
        self.camera_info_topic = rospy.get_param('~camera_info_topic', '/kinect2/hd/camera_info')

        self.h_matrix = rospy.get_param("~calibration_matrix", None)

        if self.h_matrix is not None:
            rospy.loginfo('Loaded calibration from param.')
            self.h_matrix = np.matrix(ast.literal_eval(self.h_matrix))

        self.rpm = rospy.get_param('rpm')
        self.scene_size = rospy.get_param("scene_size")
        self.scene_origin = rospy.get_param("scene_origin")

        rospy.loginfo("Projector '" + self.proj_id + "', on screen " + str(self.screen))

        img_path = rospkg.RosPack().get_path('art_projected_gui') + '/imgs'
        self.checkerboard_img = QtGui.QPixmap(img_path + "/pattern.png")
        self.calibrating = False

        desktop = QtGui.QDesktopWidget()
        geometry = desktop.screenGeometry(self.screen)
        self.move(geometry.left(), geometry.top())
        self.resize(geometry.width(), geometry.height())

        self.tfl = None
        self.bridge = CvBridge()

        self.setAutoFillBackground(True)
        p = self.palette()
        p.setColor(self.backgroundRole(), QtCore.Qt.black)
        self.setPalette(p)

        self.pix_label = QtGui.QLabel(self)
        self.pix_label.setAlignment(QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter)
        # self.pix_label.setScaledContents(True)
        self.pix_label.resize(self.size())

        self.server = rospy.get_param("scene_server")
        self.port = rospy.get_param("scene_server_port")
        self.tcpSocket = QtNetwork.QTcpSocket(self)
        self.blockSize = 0
        self.tcpSocket.readyRead.connect(self.getScene)
        self.tcpSocket.error.connect(self.on_error)
        self.tcpSocket.connectToHost(self.server, self.port)

        self.connect()

        self.calibrated_pub = rospy.Publisher("~calibrated", Bool, queue_size=1, latch=True)
        self.calibrated_pub.publish(self.is_calibrated())

        self.projectors_calibrated_sub = rospy.Subscriber('/art/interface/projected_gui/app/projectors_calibrated',  Bool,  self.projectors_calibrated_cb,  queue_size=10)
        self.projectors_calibrated = False

        self.srv_calibrate = rospy.Service("~calibrate", Empty, self.calibrate_srv_cb)
        self.corners_pub = rospy.Publisher("~corners", PoseArray, queue_size=10, latch=True)

        QtCore.QObject.connect(self, QtCore.SIGNAL('show_chessboard'), self.show_chessboard_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('show_pix_label'), self.show_pix_label_evt)

        self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint)
        self.showFullScreen()

    def show_pix_label_evt(self, show):

        if show:
            self.pix_label.show()
        else:
            self.pix_label.hide()

    def projectors_calibrated_cb(self, msg):

        self.projectors_calibrated = msg.data
        self.emit(QtCore.SIGNAL('show_pix_label'), self.projectors_calibrated)

    def connect(self):

        r = rospy.Rate(1.0 / 5)

        while not self.tcpSocket.waitForConnected(1):

            if rospy.is_shutdown():
                return
            rospy.loginfo("Waiting for scene server...")
            self.tcpSocket.connectToHost(self.server, self.port)
            r.sleep()

        rospy.loginfo('Connected to scene server.')

    def on_error(self):

        QtCore.QTimer.singleShot(0, self.connect)

    def getScene(self):

        instr = QtCore.QDataStream(self.tcpSocket)
        instr.setVersion(QtCore.QDataStream.Qt_4_0)

        while True:

            if self.blockSize == 0:
                if self.tcpSocket.bytesAvailable() < 4:
                    return

                self.blockSize = instr.readUInt32()

            if self.tcpSocket.bytesAvailable() < self.blockSize:
                return

            self.blockSize = 0

            pix = QtGui.QImage()
            ba = QtCore.QByteArray()
            instr >> ba

            if self.tcpSocket.bytesAvailable() > 0:
                rospy.logdebug("Image dropped")
                continue

            ba = QtCore.qUncompress(ba)
            if not pix.loadFromData(ba):

                rospy.logerr("Failed to load image from received data")

            if not self.is_calibrated() or self.calibrating or not self.projectors_calibrated:
                return

            img = pix.convertToFormat(QtGui.QImage.Format_ARGB32)
            img = img.mirrored()
            v = qimage2ndarray.rgb_view(img)

            # TODO gpu
            image_np = cv2.warpPerspective(v, self.h_matrix, (self.width(), self.height()), flags=cv2.INTER_NEAREST)

            height, width, channel = image_np.shape<commit_msg>disable cursor<commit_after>#!/usr/bin/env python

from PyQt4 import QtGui, QtCore, QtNetwork
import rospkg
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image, CameraInfo
import rospy
import cv2
import numpy as np
import qimage2ndarray
from std_msgs.msg import Bool
from std_srvs.srv import Empty, EmptyResponse
import message_filters
from image_geometry import PinholeCameraModel
from geometry_msgs.msg import PointStamped, Pose, PoseArray
import tf
import ast

# TODO create ProjectorROS (to separate QT / ROS stuff)
# podle vysky v pointcloudu / pozice projektoru se vymaskuji mista kde je neco vyssiho - aby se promitalo jen na plochu stolu ????


class Projector(QtGui.QWidget):

    def __init__(self):

        super(Projector, self).__init__()

        self.proj_id = rospy.get_param('~projector_id', 'test')
        self.world_frame = rospy.get_param('~world_frame', 'marker')
        self.screen = rospy.get_param('~screen_number', 0)
        self.camera_image_topic = rospy.get_param('~camera_image_topic', '/kinect2/hd/image_color_rect')
        self.camera_depth_topic = rospy.get_param('~camera_depth_topic', '/kinect2/hd/image_depth_rect')
        self.camera_info_topic = rospy.get_param('~camera_info_topic', '/kinect2/hd/camera_info')

        self.h_matrix = rospy.get_param("~calibration_matrix", None)

        if self.h_matrix is not None:
            rospy.loginfo('Loaded calibration from param.')
            self.h_matrix = np.matrix(ast.literal_eval(self.h_matrix))

        self.rpm = rospy.get_param('rpm')
        self.scene_size = rospy.get_param("scene_size")
        self.scene_origin = rospy.get_param("scene_origin")

        rospy.loginfo("Projector '" + self.proj_id + "', on screen " + str(self.screen))

        img_path = rospkg.RosPack().get_path('art_projected_gui') + '/imgs'
        self.checkerboard_img = QtGui.QPixmap(img_path + "/pattern.png")
        self.calibrating = False

        desktop = QtGui.QDesktopWidget()
        geometry = desktop.screenGeometry(self.screen)
        self.move(geometry.left(), geometry.top())
        self.resize(geometry.width(), geometry.height())

        self.tfl = None
        self.bridge = CvBridge()

        self.setAutoFillBackground(True)
        p = self.palette()
        p.setColor(self.backgroundRole(), QtCore.Qt.black)
        self.setPalette(p)

        self.pix_label = QtGui.QLabel(self)
        self.pix_label.setAlignment(QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter)
        # self.pix_label.setScaledContents(True)
        self.pix_label.resize(self.size())

        self.server = rospy.get_param("scene_server")
        self.port = rospy.get_param("scene_server_port")
        self.tcpSocket = QtNetwork.QTcpSocket(self)
        self.blockSize = 0
        self.tcpSocket.readyRead.connect(self.getScene)
        self.tcpSocket.error.connect(self.on_error)
        self.tcpSocket.connectToHost(self.server, self.port)

        self.connect()

        self.calibrated_pub = rospy.Publisher("~calibrated", Bool, queue_size=1, latch=True)
        self.calibrated_pub.publish(self.is_calibrated())

        self.projectors_calibrated_sub = rospy.Subscriber('/art/interface/projected_gui/app/projectors_calibrated',  Bool,  self.projectors_calibrated_cb,  queue_size=10)
        self.projectors_calibrated = False

        self.srv_calibrate = rospy.Service("~calibrate", Empty, self.calibrate_srv_cb)
        self.corners_pub = rospy.Publisher("~corners", PoseArray, queue_size=10, latch=True)

        QtCore.QObject.connect(self, QtCore.SIGNAL('show_chessboard'), self.show_chessboard_evt)
        QtCore.QObject.connect(self, QtCore.SIGNAL('show_pix_label'), self.show_pix_label_evt)

        self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint)
        self.showFullScreen()
        self.setCursor(QtCore.Qt.BlankCursor)

    def show_pix_label_evt(self, show):

        if show:
            self.pix_label.show()
        else:
            self.pix_label.hide()

    def projectors_calibrated_cb(self, msg):

        self.projectors_calibrated = msg.data
        self.emit(QtCore.SIGNAL('show_pix_label'), self.projectors_calibrated)

    def connect(self):

        r = rospy.Rate(1.0 / 5)

        while not self.tcpSocket.waitForConnected(1):

            if rospy.is_shutdown():
                return
            rospy.loginfo("Waiting for scene server...")
            self.tcpSocket.connectToHost(self.server, self.port)
            r.sleep()

        rospy.loginfo('Connected to scene server.')

    def on_error(self):

        QtCore.QTimer.singleShot(0, self.connect)

    def getScene(self):

        instr = QtCore.QDataStream(self.tcpSocket)
        instr.setVersion(QtCore.QDataStream.Qt_4_0)

        while True:

            if self.blockSize == 0:
                if self.tcpSocket.bytesAvailable() < 4:
                    return

                self.blockSize = instr.readUInt32()

            if self.tcpSocket.bytesAvailable() < self.blockSize:
                return

            self.blockSize = 0

            pix = QtGui.QImage()
            ba = QtCore.QByteArray()
            instr >> ba

            if self.tcpSocket.bytesAvailable() > 0:
                rospy.logdebug("Image dropped")
                continue

            ba = QtCore.qUncompress(ba)
            if not pix.loadFromData(ba):

                rospy.logerr("Failed to load image from received data")

            if not self.is_calibrated() or self.calibrating or not self.projectors_calibrated:
                return

            img = pix.convertToFormat(QtGui.QImage.Format_ARGB32)
            img = img.mirrored()
            v = qimage2ndarray.rgb_view(img)

            # TODO gpu
            image_np = cv2.warpPerspective(v, self.h_matrix, (self.width(), self.height()))

            height, width, channel = image_np.shape
{'fraction_non_alphanumeric': 0.1062339880444065, 'fraction_numerical': 0.005209222886421862, 'mean_word_length': 2.7036685641998734, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 9, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6610828', 'n_tokens_mistral': 3927, 'n_tokens_neox': 3700, 'n_words': 642}