diff --git a/src/drivers/cameraserver_py/CMakeLists.txt b/src/drivers/cameraserver_py/CMakeLists.txt new file mode 100644 index 000000000..f97c9cd1e --- /dev/null +++ b/src/drivers/cameraserver_py/CMakeLists.txt @@ -0,0 +1,19 @@ +configure_file( + cameraserver_py.in + cameraserver_py + @ONLY +) +## INSTALL ## + +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/cameraserver_py + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ + DESTINATION bin + COMPONENT cameraserver-python +) + + +INSTALL(FILES *.py DESTINATION share/jderobot/python/cameraserver_py/ COMPONENT cameraserver-python) + + +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/cameraserver_py.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT cameraserver-python) diff --git a/src/drivers/cameraserver_py/ImageProviderI.py b/src/drivers/cameraserver_py/ImageProviderI.py new file mode 100644 index 000000000..44c11fbf7 --- /dev/null +++ b/src/drivers/cameraserver_py/ImageProviderI.py @@ -0,0 +1,82 @@ +import jderobot +import threading +import os +from threadImage import ThreadImage #camera isn a threadImage +import numpy as np + +class WorkQueue(threading.Thread): + def __init__(self): + self.callbacks = [] + threading.Thread.__init__(self) + + + def run(self): + if not len(self.callbacks) == 0: + self.callbacks[0].execute() + del self.callbacks[0] + + def add(self, job): + self.callbacks.append(job) + self.run() + + +class Job(object): + + def __init__(self,cb,format, camera): + self.cb = cb + self.format = format + self.camera = camera + self.imageData = jderobot.ImageData() + + def execute(self): + if not self.getData(): + print "No data" + return + self.cb.ice_response(self.imageData) + + def getData(self): + img = self.camera.get_image() + self.imageData = jderobot.ImageData() + self.imageData.description = jderobot.ImageDescription() + self.imageData.description.width = img.shape[1] + self.imageData.description.height = img.shape[0] + self.imageData.description.format = 'RGB8' + self.pixelData = np.getbuffer(img) + + self.imageData.pixelData = self.pixelData + + return True + +class ImageProviderI(jderobot.Camera): + + def __init__(self,workQueue, camera): + self.workQueue = workQueue + self.camera = camera + + def getCameraDescription(self, current=None): + return jderobot.CameraDescription() + + def setCameraDescription(self, description): + return 0 + + def startCameraStreaming(self, current=None): + return '' + + def stopCameraStreaming(self,current=None): + print('') + + def reset(self): + print('---') + + def getImageDescription(self,current=None): + + self.imageData = jderobot.ImageDescription() + img = self.camera.get_image() + self.imageData.width = img.shape[1] + self.imageData.height = img.shape[0] + self.format = 'RGB' + return self.imageData + + def getImageData_async(self,cb,formato,curren=None): + job = Job(cb,formato, self.camera) + return self.workQueue.add(job) \ No newline at end of file diff --git a/src/drivers/cameraserver_py/cameraserver.py b/src/drivers/cameraserver_py/cameraserver.py new file mode 100644 index 000000000..eaba8f6a3 --- /dev/null +++ b/src/drivers/cameraserver_py/cameraserver.py @@ -0,0 +1,43 @@ +import Ice +import sys +import config +import cv2 +from ImageProviderI import ImageProviderI,WorkQueue +from threadImage import ThreadImage + + + +if __name__== "__main__": + + try: + cfg = config.load(sys.argv[1]) + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("cameraServer.Proxy") + fps = cfg.getPropertyWithDefault("cameraServer.FrameRate",12) + uri = cfg.getProperty("cameraServer.Uri") + name = cfg.getProperty("cameraServer.Name") + + camera = cv2.VideoCapture(uri) + + workQueue = WorkQueue() + workQueue.setDaemon(True) + + imageThread = ThreadImage(camera, fps) + + imageThread.start() + workQueue.start() + + adapter = ic.createObjectAdapterWithEndpoints("youtubeServer",endpoint) #Properties + object = ImageProviderI(workQueue, imageThread) + adapter.add(object, Ice.stringToIdentity(name)) + adapter.activate() + workQueue.join() + print name, ":", endpoint + ic.waitForShutdown() + except KeyboardInterrupt: + imageThread.stop() + del(camera) + del(ic) + sys.exit() \ No newline at end of file diff --git a/src/drivers/cameraserver_py/cameraserver.yml b/src/drivers/cameraserver_py/cameraserver.yml new file mode 100644 index 000000000..0a02a4de1 --- /dev/null +++ b/src/drivers/cameraserver_py/cameraserver.yml @@ -0,0 +1,5 @@ +cameraServer: + Proxy: "default -h 0.0.0.0 -p 9999" + Uri: 0 #0 corresponds to /dev/video0, 1 to /dev/video1, and so on... + FrameRate: 12 #FPS readed from device + Name: cameraA \ No newline at end of file diff --git a/src/drivers/cameraserver_py/cameraserver_py.in b/src/drivers/cameraserver_py/cameraserver_py.in new file mode 100644 index 000000000..2845713d2 --- /dev/null +++ b/src/drivers/cameraserver_py/cameraserver_py.in @@ -0,0 +1,3 @@ +#!/bin/bash + +python @CMAKE_INSTALL_PREFIX@/share/jderobot/python/cameraserver_py/cameraserver.py $* \ No newline at end of file diff --git a/src/drivers/cameraserver_py/threadImage.py b/src/drivers/cameraserver_py/threadImage.py new file mode 100644 index 000000000..50698a70a --- /dev/null +++ b/src/drivers/cameraserver_py/threadImage.py @@ -0,0 +1,73 @@ +# +# 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 +import cv2 + + +class ThreadImage(threading.Thread): + + def __init__(self, camera, fps=12): + self.camera = camera + self._kill_event = threading.Event() + self._image = None + self._time_cycle = 1000/fps #time_cycle in ms + self._lock = threading.Lock() + threading.Thread.__init__(self, args=self._kill_event) + self._kill_event.clear() + + self.__read_image() + + def run(self): + while (not self._kill_event.is_set()): + start_time = datetime.now() + + self.__read_image() + + 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 < self._time_cycle): + time.sleep((self._time_cycle - ms) / 1000.0) + + def __read_image(self): + retval, img = self.camera.read() + imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + self._lock.acquire() + self._image = imgRGB + self._lock.release() + + def get_image(self, format="RGB"): + self._lock.acquire() + img = self._image + self._lock.release() + return img + + + def stop(self): + ''' + Stops the client. If client is stopped you can not start again, Threading.Thread raised error + + ''' + self._kill_event.set() +