From 2871d3d9d113999edcdc94b3fb7984fad07c85b1 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Fri, 19 Jan 2018 16:56:52 +0100 Subject: [PATCH] Added rgbdViewer_py --- .../rgbdViewer_py/interfaces/__init__.py | 0 .../rgbdViewer_py/interfaces/cameraClient.py | 129 ++++++++++++++++++ .../rgbdViewer_py/interfaces/threadSensor.py | 46 +++++++ src/tools/rgbdViewer_py/rgbdViewer.py | 36 +++++ src/tools/rgbdViewer_py/rgbdViewer_py.cfg | 5 + 5 files changed, 216 insertions(+) create mode 100644 src/tools/rgbdViewer_py/interfaces/__init__.py create mode 100644 src/tools/rgbdViewer_py/interfaces/cameraClient.py create mode 100644 src/tools/rgbdViewer_py/interfaces/threadSensor.py create mode 100644 src/tools/rgbdViewer_py/rgbdViewer.py create mode 100644 src/tools/rgbdViewer_py/rgbdViewer_py.cfg diff --git a/src/tools/rgbdViewer_py/interfaces/__init__.py b/src/tools/rgbdViewer_py/interfaces/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/tools/rgbdViewer_py/interfaces/cameraClient.py b/src/tools/rgbdViewer_py/interfaces/cameraClient.py new file mode 100644 index 000000000..3102989b3 --- /dev/null +++ b/src/tools/rgbdViewer_py/interfaces/cameraClient.py @@ -0,0 +1,129 @@ +# +# 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 +# + +import traceback +import jderobot +import numpy as np +import threading +import Ice +from interfaces.threadSensor import ThreadSensor + + +class Camera: + + def __init__(self, ic, prefix): + self.lock = threading.Lock() + + try: + basecamera = ic.propertyToProxy(prefix+".Proxy") + self.proxy = jderobot.CameraPrx.checkedCast(basecamera) + prop = ic.getProperties() + self.imgFormat = prop.getProperty(prefix+".Format") + if not self.imgFormat: + self.imgFormat = "RGB8" + + self.update() + + if not self.proxy: + print ('Interface ' + prefix + ' not configured') + + except Ice.ConnectionRefusedException: + print(prefix + ': connection refused') + + except: + traceback.print_exc() + exit(-1) + + def update(self): + if self.hasproxy(): + image = self.proxy.getImageData(self.imgFormat) + height = image.description.height + width = image.description.width + + self.lock.acquire() + self.image = image + self.height = height + self.width = width + self.lock.release() + + def getImage(self): + if self.hasproxy(): + self.lock.acquire() + img = np.zeros((self.height, self.width, 3), np.uint8) + img = np.frombuffer(self.image.pixelData, dtype=np.uint8) + img.shape = self.height, self.width, 3 + + self.lock.release() + return img + + return None + + def getHeight(self): + if self.hasproxy(): + self.lock.acquire() + tmp = self.height + self.lock.release() + return tmp + return None + + def getWidth(self): + if self.hasproxy(): + self.lock.acquire() + tmp = self.width + self.lock.release() + return tmp + return None + + def hasproxy (self): + return hasattr(self,"proxy") and self.proxy + + + + +class CameraClient: + def __init__(self,ic,prefix): + self.camera = Camera(ic,prefix) + + self.kill_event = threading.Event() + self.thread = ThreadSensor(self.camera, self.kill_event) + #self.thread = ThreadSensor(self.camera) + self.thread.daemon = True + + 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 getImage(self): + return self.camera.getImage() + + def getHeight(self): + return self.camera.getHeight() + + def getWidth(self): + return self.camera.getWidth() + + def hasproxy (self): + return self.camera.hasproxy() diff --git a/src/tools/rgbdViewer_py/interfaces/threadSensor.py b/src/tools/rgbdViewer_py/interfaces/threadSensor.py new file mode 100644 index 000000000..e30ece981 --- /dev/null +++ b/src/tools/rgbdViewer_py/interfaces/threadSensor.py @@ -0,0 +1,46 @@ +# +# 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 +# Aitor Martinez Fernandez +# +import threading +import time +from datetime import datetime + +time_cycle = 80 + + +class ThreadSensor(threading.Thread): + + def __init__(self, sensor, kill_event): + self.sensor = sensor + self.kill_event = kill_event + threading.Thread.__init__(self, args=kill_event) + + def run(self): + while (not self.kill_event.is_set()): + start_time = datetime.now() + + self.sensor.update() + + finish_Time = datetime.now() + + dt = finish_Time - start_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + #print (ms) + if (ms < time_cycle): + time.sleep((time_cycle - ms) / 1000.0) diff --git a/src/tools/rgbdViewer_py/rgbdViewer.py b/src/tools/rgbdViewer_py/rgbdViewer.py new file mode 100644 index 000000000..10637bcf3 --- /dev/null +++ b/src/tools/rgbdViewer_py/rgbdViewer.py @@ -0,0 +1,36 @@ +import sys, traceback, Ice +import easyiceconfig as EasyIce +import jderobot +import numpy as np +import threading +from interfaces.cameraClient import CameraClient + +import cv2 + + +if __name__ == "__main__": + ic = EasyIce.initialize(sys.argv) + properties = ic.getProperties() + cameraRGB = CameraClient(ic, "rgbdViewer.CameraRGB") + cameraDepth = CameraClient(ic, "rgbdViewer.CameraDEPTH") + + key=-1 + while key != 1048689: + rgb = cameraRGB.getImage() + depth = cameraDepth.getImage() + + image = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + cv2.imshow("RGB", image) + + + layers = cv2.split(depth) + colord = cv2.cvtColor(layers[0],cv2.COLOR_GRAY2BGR) + cv2.imshow("DEPTH", colord) + + key=cv2.waitKey(30) + + cameraDepth.stop() + cameraRGB.stop() + + + diff --git a/src/tools/rgbdViewer_py/rgbdViewer_py.cfg b/src/tools/rgbdViewer_py/rgbdViewer_py.cfg new file mode 100644 index 000000000..f03bc235d --- /dev/null +++ b/src/tools/rgbdViewer_py/rgbdViewer_py.cfg @@ -0,0 +1,5 @@ +rgbdViewer.CameraRGB.Fps=10 +rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9999 + +rgbdViewer.CameraDEPTH.Fps=10 +rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9999