diff --git a/src/drivers/piCamServer/CMakeLists.txt b/src/drivers/piCamServer/CMakeLists.txt new file mode 100644 index 000000000..f66bce304 --- /dev/null +++ b/src/drivers/piCamServer/CMakeLists.txt @@ -0,0 +1,22 @@ +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 .py +FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*py) +FOREACH(header ${HEADERS_FILES}) + INSTALL(FILES ${header} DESTINATION share/jderobot/python/cameraserver_py/ COMPONENT cameraserver-python) +ENDFOREACH(header) + + +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/cameraserver_py.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT cameraserver-python) diff --git a/src/drivers/piCamServer/ImageProviderI.py b/src/drivers/piCamServer/ImageProviderI.py new file mode 100644 index 000000000..51f907915 --- /dev/null +++ b/src/drivers/piCamServer/ImageProviderI.py @@ -0,0 +1,95 @@ +import jderobot +import threading +import os +from threadImage import ThreadImage #camera isn a threadImage +import numpy as np +from picamera.array import PiRGBArray +from picamera import PiCamera + +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, uri): + self.cb = cb + self.format = format + self.camera = camera + self.imageData = jderobot.ImageData() + self.uri = uri + 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() + + if self.uri == 2: + self.imageData.description.width = 640 + self.imageData.description.height = 480 + else: + self.imageData.description.width = img.shape[1] + self.imageData.description.height = img.shape[0] + + self.pixelData = np.getbuffer(img) + self.imageData.description.format = 'RGB8' + self.imageData.pixelData = self.pixelData + + return True + +class ImageProviderI(jderobot.Camera): + + def __init__(self,workQueue, camera, uri): + self.workQueue = workQueue + self.camera = camera + self.uri = uri + + 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() + if self.uri == 2: + self.imageData.width = 640 + self.imageData.height = 480 + else: + 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,self.uri) + return self.workQueue.add(job) diff --git a/src/drivers/piCamServer/picamserver.py b/src/drivers/piCamServer/picamserver.py new file mode 100644 index 000000000..39535d429 --- /dev/null +++ b/src/drivers/piCamServer/picamserver.py @@ -0,0 +1,51 @@ +import Ice +import sys +import config +import cv2 +from ImageProviderI import ImageProviderI,WorkQueue +from threadImage import ThreadImage +import imutils + +if __name__== "__main__": + + try: + cfg = config.load(sys.argv[1]) + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("piCamServer.Proxy") + fps = cfg.getProperty("piCamServer.FrameRate") + uri = cfg.getProperty("piCamServer.Uri") # uri = 2 ==> PiCamera + name = cfg.getProperty("piCamServer.Name") + + if uri == 2: + #from imutils.video.pivideostream import PiVideoStream + from picamera import PiCamera + + camera = PiCamera() # Por defecto se conectara al banco 0, el unico que tenemos para la PiCam + camera.resolution = (640, 480) + camera.framerate = 32 + else: + camera = cv2.VideoCapture(uri) + + workQueue = WorkQueue() + workQueue.setDaemon(True) + + imageThread = ThreadImage(camera, fps, uri) + + imageThread.start() + workQueue.start() + + adapter = ic.createObjectAdapterWithEndpoints("youtubeServer",endpoint) #Properties + object = ImageProviderI(workQueue, imageThread, uri) + 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() diff --git a/src/drivers/piCamServer/picamserver.yml b/src/drivers/piCamServer/picamserver.yml new file mode 100644 index 000000000..cfd5222d5 --- /dev/null +++ b/src/drivers/piCamServer/picamserver.yml @@ -0,0 +1,5 @@ +piCamServer: + Proxy: "default -h 0.0.0.0 -p 9999" + Uri: 0 #0 corresponds to /dev/video0, 1 to /dev/video1, and 2 to PiCamera + FrameRate: 12 #FPS readed from device + Name: cameraA diff --git a/src/drivers/piCamServer/threadImage.py b/src/drivers/piCamServer/threadImage.py new file mode 100644 index 000000000..4c3e62854 --- /dev/null +++ b/src/drivers/piCamServer/threadImage.py @@ -0,0 +1,81 @@ +# +# 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 +from picamera.array import PiRGBArray +from picamera import PiCamera + +class ThreadImage(threading.Thread): + + def __init__(self, camera, fps, uri): + self.camera = camera + self.uri = uri + 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): + if self.uri == 2: + rawCapture = PiRGBArray(self.camera, size=(640, 480)) + self.camera.capture(rawCapture, format="rgb") + imgRGB = rawCapture.array + else: + 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() +