cameraserver_py

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)

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)

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() 

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

src/drivers/cameraserver_py/cameraserver_py.in

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+python @CMAKE_INSTALL_PREFIX@/share/jderobot/python/cameraserver_py/cameraserver.py $*

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 <almartinflorido@gmail.com>
+#       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+#
+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()
+