From 030886d5bc7c751d67d5091d3ffd246a95e082fe Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 5 Apr 2018 14:01:49 +0200 Subject: [PATCH 1/3] Updated cameraserverpy to full ROS --- src/drivers/cameraserver_py/ImageProviderI.py | 82 ------------------- src/drivers/cameraserver_py/cameraserver.py | 76 +++++++++-------- .../cameraserver_py/cameraserver_py.yml | 4 +- src/drivers/cameraserver_py/serverImage.py | 39 +++++++++ src/drivers/cameraserver_py/threadImage.py | 73 ----------------- 5 files changed, 82 insertions(+), 192 deletions(-) delete mode 100644 src/drivers/cameraserver_py/ImageProviderI.py create mode 100644 src/drivers/cameraserver_py/serverImage.py delete mode 100644 src/drivers/cameraserver_py/threadImage.py diff --git a/src/drivers/cameraserver_py/ImageProviderI.py b/src/drivers/cameraserver_py/ImageProviderI.py deleted file mode 100644 index 44c11fbf7..000000000 --- a/src/drivers/cameraserver_py/ImageProviderI.py +++ /dev/null @@ -1,82 +0,0 @@ -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 index eaba8f6a3..641db0b50 100644 --- a/src/drivers/cameraserver_py/cameraserver.py +++ b/src/drivers/cameraserver_py/cameraserver.py @@ -1,43 +1,49 @@ -import Ice import sys import config import cv2 -from ImageProviderI import ImageProviderI,WorkQueue -from threadImage import ThreadImage +import rospy +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError +from serverImage import ServerImage +img_formats = ["bmp", "dib", "gif", "jpeg", "jpg", "jpe", "jp2", "png", "pbm", "pgm", "ppm", "sr", "ras", "tiff", "tif"] +video_formats = ["mp4", "avi"] + if __name__== "__main__": + cfg = config.load(sys.argv[1]) + + topic = cfg.getProperty("cameraServer.Topic") + fps = cfg.getPropertyWithDefault("cameraServer.FrameRate",12) + uri = cfg.getProperty("cameraServer.Uri") + name = cfg.getProperty("cameraServer.Name") + + if type(uri) == str and "." in uri and uri.split(".")[-1] in img_formats: + print "loading image: ", uri.split("/")[-1] + camera = ServerImage(uri) + elif type(uri) == int: + print "loading camera ..." + camera = cv2.VideoCapture(uri) + elif type(uri) == str and "." in uri and uri.split(".")[-1] in video_formats: + print "loading video: ", uri.split("/")[-1] + camera = cv2.VideoCapture(uri) + else: + print "[Format error]" + sys.exit() + + bridge = CvBridge() + pub = rospy.Publisher(topic, Image, queue_size=1) + rospy.init_node(name) + r = rospy.Rate(fps) + + while not rospy.is_shutdown(): + retval, img = camera.read() + imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + msg = bridge.cv2_to_imgmsg(imgRGB, "rgb8") + + pub.publish(msg) + r.sleep() + + - 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_py.yml b/src/drivers/cameraserver_py/cameraserver_py.yml index 8c1740a5e..e85aa3871 100644 --- a/src/drivers/cameraserver_py/cameraserver_py.yml +++ b/src/drivers/cameraserver_py/cameraserver_py.yml @@ -1,5 +1,5 @@ cameraServer: - Proxy: "default -h 0.0.0.0 -p 9999" + Topic: "camera/rgb" Uri: 0 #0 corresponds to /dev/video0, 1 to /dev/video1, and so on... FrameRate: 12 #FPS readed from device - Name: cameraA + Name: cameraserver diff --git a/src/drivers/cameraserver_py/serverImage.py b/src/drivers/cameraserver_py/serverImage.py new file mode 100644 index 000000000..7cb650354 --- /dev/null +++ b/src/drivers/cameraserver_py/serverImage.py @@ -0,0 +1,39 @@ +# +# 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 +# Francisco Perez Salgado +# + +import threading +import time +import cv2 + + +class ServerImage(): + + def __init__(self, uri): + self._image = None + self._lock = threading.Lock() + self.uri = uri + + def read(self): + image = cv2.imread(self.uri) + self._lock.acquire() + self._image = image + self._lock.release() + return 0, self._image \ No newline at end of file diff --git a/src/drivers/cameraserver_py/threadImage.py b/src/drivers/cameraserver_py/threadImage.py deleted file mode 100644 index 50698a70a..000000000 --- a/src/drivers/cameraserver_py/threadImage.py +++ /dev/null @@ -1,73 +0,0 @@ -# -# 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() - From 04abc3f00f288e431fdfb586060a8b9d6710adf8 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 11 Apr 2018 11:24:26 +0200 Subject: [PATCH 2/3] changed cameraserver topic --- src/drivers/cameraserver_py/cameraserver_py.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/cameraserver_py/cameraserver_py.yml b/src/drivers/cameraserver_py/cameraserver_py.yml index e85aa3871..dfa90899f 100644 --- a/src/drivers/cameraserver_py/cameraserver_py.yml +++ b/src/drivers/cameraserver_py/cameraserver_py.yml @@ -1,5 +1,5 @@ cameraServer: - Topic: "camera/rgb" + Topic: "cameraserver/rgb" Uri: 0 #0 corresponds to /dev/video0, 1 to /dev/video1, and so on... FrameRate: 12 #FPS readed from device Name: cameraserver From 9c20bd1bf96654370bd185b181a4af501bce172e Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 11 Apr 2018 12:08:27 +0200 Subject: [PATCH 3/3] updated cameraview.yml --- src/tools/cameraview/cameraview.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tools/cameraview/cameraview.yml b/src/tools/cameraview/cameraview.yml index 9b936ec35..5c13ae470 100644 --- a/src/tools/cameraview/cameraview.yml +++ b/src/tools/cameraview/cameraview.yml @@ -3,7 +3,7 @@ Cameraview: Server: Ice # Deactivate, Ice , ROS Proxy: "cameraA:tcp -h localhost -p 9999" Format: RGB8 - Topic: "/TurtlebotROS/cameraL/image_raw" + Topic: "/cameraserver/rgb" Name: cameraA Fps: 30