Merge pull request #851 from aitormf/tools/pantilt_teleop/821

[issue #821] Created pantilt_teleop

src/libs/parallelIce_py/parallelIce/ptMotors.py

@@ -0,0 +1,123 @@
+# -*- coding: utf-8 -*-
+#
+#  Copyright (C) 1997-2016 JDE Developers Team
+#
+#  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 3 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.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program.  If not, see http://www.gnu.org/licenses/.
+#  Authors :
+#       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+#
+
+import traceback
+import jderobot
+import threading
+import Ice
+from parallelIce.threadSensor import ThreadSensor
+
+
+class PTMotors:
+
+    def __init__(self, ic, prefix):
+        self.lock = threading.Lock()
+
+        self.data=jderobot.PTMotorsData()
+
+        self.params=jderobot.PTMotorsParams()
+        prop = ic.getProperties()
+
+        try:
+            base = ic.propertyToProxy(prefix+".Proxy")
+            self.proxy = jderobot.PTMotorsPrx.checkedCast(base)
+
+            if not self.proxy:
+                print ('Interface ' + prefix + ' not configured')
+
+            else:
+                self.params = self.proxy.getPTMotorsParams()
+
+                print ("+++ MAX/MIN Pan/Tilt Values +++")
+                print ("+    Min Pan: " + str(self.params.minPan) + "         +")
+                print ("+    Max Pan: " + str(self.params.maxPan) + "          +")
+                print ("+    Max Pan speed: " + str(self.params.maxPanSpeed) + "     +")
+                print ("+    Min Tilt: " + str(self.params.minTilt) + "         +")
+                print ("+    Max Tilt: " + str(self.params.maxTilt) + "          +")
+                print ("+    Max Tilt speed: " + str(self.params.maxTiltSpeed) + "    +")
+                print ("++++++++++++++++++++++++++++++")
+
+        except Ice.ConnectionRefusedException:
+            print(prefix + ': connection refused')
+
+        except:
+            traceback.print_exc()
+            exit(-1)
+
+    def getLimits(self):
+        self.lock.acquire()
+        params = self.params        
+        self.lock.release()
+
+        return params
+
+    def setPTMotorsData(self, pan, tilt, panspeed, tiltspeed):
+
+        self.lock.acquire()
+        self.data.pan = pan
+        self.data.tilt = tilt
+        self.data.panSpeed = panspeed
+        self.data.tiltSpeed = tiltspeed
+        self.lock.release()
+
+
+
+    def sendPTMotorsData(self):
+        if self.hasproxy():
+            self.lock.acquire()
+            self.proxy.setPTMotorsData(self.data)
+            self.lock.release()
+
+    def hasproxy (self):
+        return hasattr(self,"proxy") and self.proxy
+
+    def update(self):
+        self.sendPTMotorsData()
+
+
+
+class PTMotorsClient:
+    def __init__(self,ic,prefix, start = False):
+        self.motors = PTMotors(ic,prefix)
+
+        self.kill_event = threading.Event()
+        self.thread = ThreadSensor(self.motors, self.kill_event)
+        self.thread.daemon = True
+
+        if start:
+            self.start()
+
+    # if client is stopped you can not start again, Threading.Thread raised error
+    def start(self):
+        self.kill_event.clear()
+        self.thread.start()
+
+    # if client is stopped you can not start again
+    def stop(self):
+        self.kill_event.set()
+
+    def getLimits(self):
+        return self.motors.getLimits()
+
+    def hasproxy():
+        return self.motors.hasproxy()
+
+    def setPTMotorsData(self, pan, tilt, panspeed, tiltspeed):
+        self.motors.setPTMotorsData(pan, tilt, panspeed, tiltspeed)

src/tools/pantilt_teleop_py/CMakeLists.txt

@@ -0,0 +1,28 @@
+
+configure_file(
+    pantilt_teleop.in
+    pantilt_teleop
+    @ONLY
+)
+
+## INSTALL ##
+
+# install Launcher
+install(
+    FILES ${CMAKE_CURRENT_BINARY_DIR}/pantilt_teleop
+    PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
+    DESTINATION bin
+)
+
+# Install .py
+FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*py)
+FOREACH(header ${HEADERS_FILES})
+	INSTALL(FILES ${header} DESTINATION share/jderobot/python/pantilt_teleop_py/ COMPONENT tools)
+ENDFOREACH(header)
+
+# Install gui
+INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/gui DESTINATION share/jderobot/python/pantilt_teleop_py COMPONENT tools PATTERN .svn EXCLUDE)
+
+# Install resources
+#INSTALL (DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/resources DESTINATION share/jderobot/python/pantilt_teleop_py COMPONENT tools PATTERN .svn EXCLUDE)
+INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/pantilt_teleop.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf)

src/tools/pantilt_teleop_py/generateGUI

@@ -0,0 +1,9 @@
+#!/bin/bash
+	cd resources 
+	pyrcc5 resources.qrc -o resources_rc.py
+	mv resources_rc.py ..
+
+	cd ../gui
+	pyuic5 ui_gui.ui > ui_gui.py
+	cd ..
+

src/tools/pantilt_teleop_py/gui/GUI.py

@@ -0,0 +1,82 @@
+#
+#  Copyright (C) 1997-2016 JDE Developers Team
+#
+#  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 3 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.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program.  If not, see http://www.gnu.org/licenses/.
+#  Authors :
+#       Alberto Martin Florido <almartinflorido@gmail.com>
+#       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+#
+
+
+from PyQt5.QtCore import pyqtSignal
+from PyQt5.QtWidgets import QMainWindow
+from .ui_gui import Ui_MainWindow
+from .teleopWidget import TeleopWidget
+from .cameraWidget import CameraWidget
+from .communicator import Communicator
+from .logoWidget import LogoWidget
+
+
+class MainWindow(QMainWindow, Ui_MainWindow):
+
+    updGUI = pyqtSignal()
+
+    def __init__(self, parent=None):
+        super(MainWindow, self).__init__(parent)
+        self.setupUi(self)
+        self.teleop = TeleopWidget(self)
+        self.tlLayout.addWidget(self.teleop)
+        self.teleop.setVisible(True)
+
+        self.logo = LogoWidget(self, self.logoLayout.parent().width(), self.logoLayout.parent().height())
+        self.logoLayout.addWidget(self.logo)
+        self.logo.setVisible(True)
+
+        self.updGUI.connect(self.updateGUI)
+
+        self.cameraWidget = CameraWidget(self)
+
+        self.cameraCommunicator = Communicator()
+        self.trackingCommunicator = Communicator()
+
+    def setCamera(self, camera):
+        self.camera = camera
+        self.cameraWidget.show()
+
+    def getCamera(self):
+        return self.camera
+
+    def setMotors(self, motors):
+        self.motors = motors
+
+    def getMotors(self):
+        return self.motors
+
+    def updateGUI(self):
+        self.cameraWidget.imageUpdate.emit()
+
+    def setXYValues(self, newX, newY):
+        limits = self.motors.getLimits()
+        pan =  newX*limits.maxPan
+        tilt = - newY*limits.maxTilt
+
+
+
+        self.YValue.setText(str(tilt))
+        self.XValue.setText(str(pan))
+        #self.YValue.setText("{:.0f}".format(tilt))
+        #self.XValue.setText("{:.0f}".format(pan))
+        if (self.motors):
+            self.motors.setPTMotorsData(pan, tilt, limits.maxPanSpeed, limits.maxTiltSpeed)
+

src/tools/pantilt_teleop_py/gui/__init__.py

@@ -0,0 +1 @@
+# -*- coding: utf-8 -*-

src/tools/pantilt_teleop_py/gui/cameraWidget.py

@@ -0,0 +1,53 @@
+#
+#  Copyright (C) 1997-2016 JDE Developers Team
+#
+#  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 3 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.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program.  If not, see http://www.gnu.org/licenses/.
+#  Authors :
+#       Alberto Martin Florido <almartinflorido@gmail.com>
+#       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+#
+from PyQt5 import QtWidgets, QtCore
+from PyQt5.QtGui import QImage, QPixmap
+
+
+class CameraWidget(QtWidgets.QWidget):
+
+    imageUpdate = QtCore.pyqtSignal()
+
+    def __init__(self, winParent):
+        super(CameraWidget, self).__init__()
+        self.winParent = winParent
+        self.imageUpdate.connect(self.updateImage)
+        self.initUI()
+
+    def initUI(self):
+
+        self.setWindowTitle("Camera")
+        self.setMinimumSize(100,100)
+
+        self.imgLabel = QtWidgets.QLabel(self)
+        self.imgLabel.show()
+
+    def updateImage(self):
+
+        if (self.winParent.getCamera()):
+            img = self.winParent.getCamera().getImage()
+            if img is not None:
+                image = QImage(img.data, img.shape[1], img.shape[0],
+                                     img.shape[1] * img.shape[2], QImage.Format_RGB888)
+
+                size = QtCore.QSize(img.shape[1], img.shape[0])
+                self.resize(size)
+                self.imgLabel.resize(size)
+                self.imgLabel.setPixmap(QPixmap.fromImage(image))

src/tools/pantilt_teleop_py/gui/communicator.py

@@ -0,0 +1,24 @@
+#
+#  Copyright (C) 1997-2015 JDE Developers Team
+#
+#  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 3 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.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program.  If not, see http://www.gnu.org/licenses/.
+#  Authors :
+#       Alberto Martin Florido <almartinflorido@gmail.com>
+#
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+from PyQt5 import QtCore
+class Communicator(QtCore.QObject):
+    updateBW = QtCore.pyqtSignal()

src/tools/pantilt_teleop_py/gui/logoWidget.py

@@ -0,0 +1,44 @@
+#
+#  Copyright (C) 1997-2015 JDE Developers Team
+#
+#  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 3 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.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program.  If not, see http://www.gnu.org/licenses/.
+#  Authors :
+#       Alberto Martin Florido <almartinflorido@gmail.com>
+#
+import resources_rc
+from PyQt5 import QtGui
+from PyQt5.QtCore import pyqtSignal, QPointF, Qt, QPoint
+from PyQt5.QtWidgets import QWidget, QGridLayout
+
+class LogoWidget(QWidget):
+
+    def __init__(self,winParent, width=0, height=0):    
+        super(LogoWidget, self).__init__()
+        self.winParent=winParent
+        qimage=QtGui.QImage()
+        qimage.load(':images/jderobot.svg')
+        if (width != 0 and height != 0):
+        	self.qimage = qimage.scaled(0.8*width, 0.8*height, Qt.KeepAspectRatio)
+        	#self.qimage = qimage.scaled(0.8*width, 0.8*height)
+        	self.resize(width, height)
+        else:
+        	self.qimage = qimage
+
+
+    def paintEvent(self, e):
+
+        painter=QtGui.QPainter(self)
+        painter.drawImage(self.width()/2-self.qimage.width()/2, self.height()/2-self.qimage.height()/2, self.qimage)
+
+