[#1116 #1095] CameraServer_py is full ros and full opencv

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

src/drivers/cameraserver_py/cameraserver_py.yml

@@ -1,5 +1,5 @@
 cameraServer:
-  Proxy: "default -h 0.0.0.0 -p 9999"
+  Topic: "cameraserver/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

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 <almartinflorido@gmail.com>
+#       Aitor Martinez Fernandez <aitor.martinez.fernandez@gmail.com>
+#		Francisco Perez Salgado <f.perez475@gmail.com>
+#
+
+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

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