Frivas 761 sync rgbd

src/drivers/cameraserver/cameraserver.cpp

@@ -44,8 +44,8 @@
 
 #include <zlib.h>
 #include <logger/Logger.h>
-#include <jderobotutil/CameraHandler.h>
-#include <jderobotutil/CameraTask.h>
+#include <jderobotutil/interfaceHandlers/CameraHandler.h>
+#include <jderobotutil/interfaceHandlers/CameraTask.h>
 #include <ns/ns.h>
 
 #include "easyiceconfig/EasyIce.h" 

src/drivers/openniServer/CMakeLists.txt

@@ -7,7 +7,17 @@ IF(OPENNI2_LIB)
 
 
 
-	SET( SOURCE_FILES openniServer.cpp OpenNiServerLib/myprogeo OpenNiServerLib/ConcurrentDevice OpenNiServerLib/OpenCVConverter OpenNiServerLib/OpenNIDeviceListener OpenNiServerLib/RGBCamera.cpp OpenNiServerLib/RGBCamera.h OpenNiServerLib/DepthCamera.cpp OpenNiServerLib/DepthCamera.h OpenNiServerLib/PointCloudServer.cpp OpenNiServerLib/PointCloudServer.h)
+	SET( SOURCE_FILES
+			openniServer.cpp
+			OpenNiServerLib/myprogeo
+			OpenNiServerLib/ConcurrentDevice
+			OpenNiServerLib/OpenCVConverter
+			OpenNiServerLib/OpenNIDeviceListener
+			OpenNiServerLib/RGBCamera
+			OpenNiServerLib/DepthCamera
+			OpenNiServerLib/PointCloudServer
+			OpenNiServerLib/RGBDServer
+			)
     add_definitions(-DGLADE_DIR="${gladedir}")
 
 	set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,-rpath ./" ) # Opciones para el compilador

src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.cpp

@@ -72,6 +72,14 @@ ConcurrentDevice::ConcurrentDevice(int fps, int cameraIdx, DeviceConfig config,
     cycle=(float)(1/(float)fps)*1000000;
 
 
+
+    openni::Status rc = g_device.setDepthColorSyncEnabled(true);
+    if (rc != openni::STATUS_OK)
+    {
+        LOG(ERROR) << "Can't frame sync";
+        return;
+    }
+
     LOG(INFO) << "Device initialized";
     return;
 
@@ -325,9 +333,10 @@ int ConcurrentDevice::openCommon()
     return 0;
 }
 
-cv::Mat ConcurrentDevice::getDepthImage() {
+cv::Mat ConcurrentDevice::getDepthImage(bool withlock) {
     openni::DepthPixel *depthPixels;
-    this->mutex.lock();
+    if (withlock)
+        this->mutex.lock();
     bool validFrame=false;
     if (g_depthFrame.isValid())
     {
@@ -336,7 +345,8 @@ cv::Mat ConcurrentDevice::getDepthImage() {
         memcpy(depthPixels, g_depthFrame.getData(),
                g_depthFrame.getHeight() * g_depthFrame.getWidth() * sizeof(openni::DepthPixel));
     }
-    this->mutex.unlock();
+    if (withlock)
+        this->mutex.unlock();
     if (validFrame) {
 
         cv::Mat depthImage = OpenCVConverter::convertDepthToCVMat((const openni::DepthPixel *) depthPixels,
@@ -358,17 +368,18 @@ cv::Mat ConcurrentDevice::getDepthImage() {
     }
 }
 
-cv::Mat ConcurrentDevice::getRGBImage() {
+cv::Mat ConcurrentDevice::getRGBImage(bool withlock) {
     openni::RGB888Pixel *rgbPixels;
     bool validFrame=false;
-    this->mutex.lock();
+    if (withlock)
+        this->mutex.lock();
     if (g_colorFrame.isValid()) {
         validFrame=true;
         rgbPixels = new openni::RGB888Pixel[g_colorFrame.getHeight() * g_colorFrame.getWidth()];
-        memcpy(rgbPixels, g_colorFrame.getData(),
-               g_colorFrame.getHeight() * g_colorFrame.getWidth() * sizeof(openni::RGB888Pixel));
+        memcpy(rgbPixels, g_colorFrame.getData(), g_colorFrame.getHeight() * g_colorFrame.getWidth() * sizeof(openni::RGB888Pixel));
     }
-    this->mutex.unlock();
+    if (withlock)
+        this->mutex.unlock();
 
     if (validFrame) {
         cv::Mat colorImage = OpenCVConverter::convertRGBToCVMat((const openni::RGB888Pixel *) rgbPixels,
@@ -442,3 +453,10 @@ VideoMode ConcurrentDevice::getVideoMode() {
     return this->videoMode;
 }
 
+void ConcurrentDevice::getSyncData(cv::Mat &rgb, cv::Mat &depth) {
+    this->mutex.lock();
+    rgb=this->getRGBImage(false);
+    depth=this->getDepthImage(false);
+    this->mutex.unlock();
+}
+

src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.h

@@ -19,8 +19,9 @@ class ConcurrentDevice:public IceUtil::Thread {
 public:
     ConcurrentDevice(int fps,int cameraIdx, DeviceConfig config, const cv::Size& size);
     ~ConcurrentDevice();
-    cv::Mat getRGBImage();
-    cv::Mat getDepthImage();
+    cv::Mat getRGBImage(bool withlock=true);
+    cv::Mat getDepthImage(bool withlock=true);
+    void getSyncData(cv::Mat& rgb, cv::Mat& depth);
     void getDistances(std::vector<int>& distances);
     virtual void run();
     void stop();

src/drivers/openniServer/OpenNiServerLib/DepthCamera.h

@@ -5,8 +5,8 @@
 #ifndef JDEROBOT_DEPTHCAMERA_H
 #define JDEROBOT_DEPTHCAMERA_H
 
-#include <jderobotutil/CameraHandler.h>
-#include <jderobotutil/CameraTask.h>
+#include <jderobotutil/interfaceHandlers/CameraHandler.h>
+#include <jderobotutil/interfaceHandlers/CameraTask.h>
 #include <opencv2/videoio.hpp>
 #include <opencv2/videoio/videoio_c.h>
 #include "ConcurrentDevice.h"

src/drivers/openniServer/OpenNiServerLib/RGBCamera.h

@@ -5,8 +5,8 @@
 #ifndef JDEROBOT_RGBCAMERA_DEVICE_H
 #define JDEROBOT_RGBCAMERA_DEVICE_H
 
-#include <jderobotutil/CameraHandler.h>
-#include <jderobotutil/CameraTask.h>
+#include <jderobotutil/interfaceHandlers/CameraHandler.h>
+#include <jderobotutil/interfaceHandlers/CameraTask.h>
 #include <opencv2/videoio.hpp>
 #include <opencv2/videoio/videoio_c.h>
 #include "ConcurrentDevice.h"

src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp

@@ -0,0 +1,103 @@
+//
+// Created by frivas on 9/05/17.
+//
+
+#include "RGBDServer.h"
+#include <jderobot/logger/Logger.h>
+#include <jderobotutil/utils/CameraUtils.h>
+#include <visionlib/colorspaces/imagecv.h>
+
+
+namespace openniServer {
+
+    RGBDServer::RGBDServer(std::string &propertyPrefix, const Ice::PropertiesPtr propIn,
+                                       ConcurrentDevicePtr device) :
+            prefix(propertyPrefix),
+            device(device) {
+        Ice::PropertiesPtr prop = propIn;
+
+        int fps = prop->getPropertyAsIntWithDefault("openniServer.pointCloud.Fps", 10);
+        bool extra = (bool) prop->getPropertyAsIntWithDefault("openniServer.ExtraCalibration", 0);
+        replyCloud = new ReplyCloud(this, device, fps);
+        this->control = replyCloud->start();
+    }
+
+    RGBDServer::~RGBDServer() {
+        LOG(INFO) << "Stopping and joining thread for pointCloud";
+        replyCloud->destroy();
+        this->control.join();
+
+    }
+
+    jderobot::rgbData RGBDServer::getData(const Ice::Current &) {
+        data = replyCloud->getData();
+        return data;
+    }
+
+
+
+    RGBDServer::ReplyCloud::ReplyCloud(RGBDServer *server, ConcurrentDevicePtr device, int fpsIn):
+            server(server),
+            device(device),
+            fps(fpsIn),
+            _done(false)
+    {
+        this->cameraSize=cv::Size(device->getVideoMode().witdh,device->getVideoMode().heigth);
+        LOG(INFO) << "Working with: " << device->getVideoMode().witdh << " x " <<device->getVideoMode().heigth;
+
+    }
+
+    void RGBDServer::ReplyCloud::run() {
+
+        std::cout << "rgbd" << std::endl;
+        while (this->cameraSize == cv::Size(0,0)){
+            LOG(WARNING) << "Trying to get valid videoMode";
+
+            this->cameraSize=cv::Size(device->getVideoMode().witdh,device->getVideoMode().heigth);
+            sleep(1);
+        }
+
+
+        int cycle;
+
+
+        cycle = (float) (1 / (float) fps) * 1000000;
+        IceUtil::Time lastIT = IceUtil::Time::now();
+        while (!(_done)) {
+            float distance;
+            cv::Mat rgb,depth;
+            this->device->getSyncData(rgb,depth);
+
+            this->mutex.lock();
+            jderobot::rgbData temporalData;
+            temporalData.color = CameraUtils::convert(rgb);
+            temporalData.depth = CameraUtils::convert(depth);
+
+            this->stableData= temporalData;
+            this->mutex.unlock();
+
+            int delay = IceUtil::Time::now().toMicroSeconds() - lastIT.toMicroSeconds();
+            if (delay > cycle) {
+                DLOG(WARNING) << "-------- openniServer: RGBD timeout";
+            } else {
+                if (delay < 1 || delay > cycle)
+                    delay = 1;
+                usleep(delay);
+            }
+
+
+            lastIT = IceUtil::Time::now();
+        }
+    }
+
+    jderobot::rgbData RGBDServer::ReplyCloud::getData(){
+        this->mutex.lock();
+        this->returnData = this->stableData;
+        this->mutex.unlock();
+        return this->returnData;
+    }
+
+    void RGBDServer::ReplyCloud::destroy() {
+        this->_done = true;
+    }
+}

src/drivers/openniServer/OpenNiServerLib/RGBDServer.h

@@ -0,0 +1,58 @@
+//
+// Created by frivas on 9/05/17.
+//
+
+#ifndef JDEROBOT_RGBDSERVER_H
+#define JDEROBOT_RGBDSERVER_H
+
+#include <Ice/Ice.h>
+#include "ConcurrentDevice.h"
+#include <jderobot/rgbd.h>
+
+
+namespace openniServer {
+
+    class RGBDServer: virtual public jderobot::rgbd {
+    public:
+        RGBDServer(std::string &propertyPrefix, const Ice::PropertiesPtr propIn, ConcurrentDevicePtr device);
+
+        virtual ~RGBDServer();
+
+
+        virtual jderobot::rgbData getData(const Ice::Current &);
+
+    private:
+        class ReplyCloud : public IceUtil::Thread {
+        public:
+            ReplyCloud(RGBDServer *server, ConcurrentDevicePtr device, int fpsIn);
+
+            void run();
+
+
+            jderobot::rgbData getData();
+
+            virtual void destroy();
+
+
+        private:
+            RGBDServer *server;
+            ConcurrentDevicePtr device;
+            int fps;
+            jderobot::rgbData stableData, returnData;
+            cv::Size cameraSize;
+            bool _done;
+            IceUtil::Mutex mutex;
+        };
+
+        typedef IceUtil::Handle <ReplyCloud> ReplyCloudPtr;
+        ReplyCloudPtr replyCloud;
+        std::string prefix;
+        jderobot::rgbData data;
+        IceUtil::ThreadControl control;
+        ConcurrentDevicePtr device;
+
+    };
+
+}
+
+#endif //JDEROBOT_RGBDSERVER_H

src/drivers/openniServer/openniServer.cfg

@@ -1,4 +1,4 @@
-#without registry
+    #without registry
 openniServer.Endpoints=default -h 0.0.0.0 -p 9999
 #with registry
 #cameras configuration
@@ -37,11 +37,15 @@ openniServer.PointCloud.Name=pointcloud1
 openniServer.pointCloud.Fps=15
 
 
+openniServer.rgbd.Name=rgbd1
+openniServer.rgbd.Fps=25
+
 
 #Other components
 openniServer.CameraRGB=1
 openniServer.CameraIR=0
 openniServer.CameraDEPTH=1
+openniServer.rgbd=1
 openniServer.pointCloudActive=1
 openniServer.Pose3DMotorsActive=0
 openniServer.KinectLedsActive=0

src/drivers/openniServer/openniServer.cpp

@@ -47,6 +47,7 @@
 #include <OpenNiServerLib/RGBCamera.h>
 #include <OpenNiServerLib/DepthCamera.h>
 #include <OpenNiServerLib/PointCloudServer.h>
+#include <OpenNiServerLib/RGBDServer.h>
 
 
 #include "easyiceconfig/EasyIce.h"
@@ -60,6 +61,8 @@ bool killed;
 openniServer::RGBCamera *camRGB;
 openniServer::DepthCamera *camDEPTH;
 openniServer::PointCloudServer *pc1;
+openniServer::RGBDServer *rgbdServer;
+
 jderobot::ns* namingService = NULL;
 ConcurrentDevicePtr device;
 
@@ -94,6 +97,7 @@ int main(int argc, char** argv){
 	int motors = prop->getPropertyAsIntWithDefault(componentPrefix + ".Pose3DMotorsActive",0);
 	int leds = prop->getPropertyAsIntWithDefault(componentPrefix + ".KinectLedsActive",0);
 	int pointCloud = prop->getPropertyAsIntWithDefault(componentPrefix + ".pointCloudActive",0);
+	bool sync_server = prop->getPropertyAsIntWithDefault(componentPrefix + ".rgbd",0);
 
     //todo
 //	mirrorDepth = prop->getPropertyAsIntWithDefault(componentPrefix + ".CameraDEPTH.Mirror",0);
@@ -189,6 +193,17 @@ int main(int argc, char** argv){
 		adapter->add(pc1 , ic->stringToIdentity(Name));
 		LOG(INFO) << "              -------- openniServer: Component: PointCloud created successfully(" + Endpoints + "@" + Name ;
 	}
+
+
+	if (sync_server){
+		std::string objPrefix(componentPrefix + ".rgbd.");
+		std::string Name = prop->getProperty(objPrefix + "Name");
+		LOG(INFO) <<  "Creating rgbd " + Name ;
+		rgbdServer = new openniServer::RGBDServer(objPrefix,prop,device);
+		adapter->add(rgbdServer , ic->stringToIdentity(Name));
+		LOG(INFO) << "              -------- openniServer: Component: rgbd created successfully(" + Endpoints + "@" + Name ;
+	}
+
 	adapter->activate();
 	ic->waitForShutdown();
 	adapter->destroy();

src/interfaces/slice/jderobot/CMakeLists.txt

@@ -32,6 +32,10 @@ FOREACH(currentSourceFile ${ICE_FILES})
 			WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/../../cpp/jderobot/
 	)
 
+	message(COMMAND slice2cpp -I${CMAKE_CURRENT_LIST_DIR}/.. -I${CMAKE_CURRENT_LIST_DIR} ${CMAKE_CURRENT_LIST_DIR}/${new_source}
+			WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/../../cpp/jderobot/
+			)
+
   # Python
 	execute_process(COMMAND slice2py -I${CMAKE_CURRENT_LIST_DIR}/.. -I${CMAKE_CURRENT_LIST_DIR} ${CMAKE_CURRENT_LIST_DIR}/${new_source} --output-dir ${CMAKE_CURRENT_LIST_DIR}/../../python/ INPUT_FILE ${currentSourceFile})