From 16ef564cda026e3a15e695a1f0397137d76d8a06 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Tue, 9 May 2017 23:02:35 +0200 Subject: [PATCH 1/3] #761 Refactored jderobot handlers and included rgbd interface --- src/drivers/cameraserver/cameraserver.cpp | 4 +- .../OpenNiServerLib/DepthCamera.h | 4 +- .../openniServer/OpenNiServerLib/RGBCamera.h | 4 +- src/interfaces/slice/jderobot/CMakeLists.txt | 4 ++ src/interfaces/slice/jderobot/rgbd.ice | 42 +++++++++++++++++++ src/libs/jderobotutil/CMakeLists.txt | 9 ++-- .../{ => interfaceHandlers}/CameraHandler.cpp | 4 +- .../{ => interfaceHandlers}/CameraHandler.h | 0 .../{ => interfaceHandlers}/CameraTask.cpp | 28 ++++--------- .../{ => interfaceHandlers}/CameraTask.h | 0 .../interfaceHandlers/RGBDHandler.cpp | 39 +++++++++++++++++ .../interfaceHandlers/RGBDHandler.h | 35 ++++++++++++++++ .../jderobotutil/{ => utils}/CameraUtils.cpp | 26 ++++++++++++ .../jderobotutil/{ => utils}/CameraUtils.h | 1 + src/libs/parallelIce/cameraClient.cpp | 2 +- .../replayer2/interfaces/ReplayerCamera.h | 4 +- 16 files changed, 171 insertions(+), 35 deletions(-) create mode 100644 src/interfaces/slice/jderobot/rgbd.ice rename src/libs/jderobotutil/{ => interfaceHandlers}/CameraHandler.cpp (95%) rename src/libs/jderobotutil/{ => interfaceHandlers}/CameraHandler.h (100%) rename src/libs/jderobotutil/{ => interfaceHandlers}/CameraTask.cpp (88%) rename src/libs/jderobotutil/{ => interfaceHandlers}/CameraTask.h (100%) create mode 100644 src/libs/jderobotutil/interfaceHandlers/RGBDHandler.cpp create mode 100644 src/libs/jderobotutil/interfaceHandlers/RGBDHandler.h rename src/libs/jderobotutil/{ => utils}/CameraUtils.cpp (87%) rename src/libs/jderobotutil/{ => utils}/CameraUtils.h (79%) diff --git a/src/drivers/cameraserver/cameraserver.cpp b/src/drivers/cameraserver/cameraserver.cpp index b83465e68..3d7744bd2 100644 --- a/src/drivers/cameraserver/cameraserver.cpp +++ b/src/drivers/cameraserver/cameraserver.cpp @@ -44,8 +44,8 @@ #include #include -#include -#include +#include +#include #include #include "easyiceconfig/EasyIce.h" diff --git a/src/drivers/openniServer/OpenNiServerLib/DepthCamera.h b/src/drivers/openniServer/OpenNiServerLib/DepthCamera.h index 93eb7b3af..4a5db0e75 100644 --- a/src/drivers/openniServer/OpenNiServerLib/DepthCamera.h +++ b/src/drivers/openniServer/OpenNiServerLib/DepthCamera.h @@ -5,8 +5,8 @@ #ifndef JDEROBOT_DEPTHCAMERA_H #define JDEROBOT_DEPTHCAMERA_H -#include -#include +#include +#include #include #include #include "ConcurrentDevice.h" diff --git a/src/drivers/openniServer/OpenNiServerLib/RGBCamera.h b/src/drivers/openniServer/OpenNiServerLib/RGBCamera.h index 6c0210024..91cca3dce 100644 --- a/src/drivers/openniServer/OpenNiServerLib/RGBCamera.h +++ b/src/drivers/openniServer/OpenNiServerLib/RGBCamera.h @@ -5,8 +5,8 @@ #ifndef JDEROBOT_RGBCAMERA_DEVICE_H #define JDEROBOT_RGBCAMERA_DEVICE_H -#include -#include +#include +#include #include #include #include "ConcurrentDevice.h" diff --git a/src/interfaces/slice/jderobot/CMakeLists.txt b/src/interfaces/slice/jderobot/CMakeLists.txt index a9aa14b00..007ab247b 100644 --- a/src/interfaces/slice/jderobot/CMakeLists.txt +++ b/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}) diff --git a/src/interfaces/slice/jderobot/rgbd.ice b/src/interfaces/slice/jderobot/rgbd.ice new file mode 100644 index 000000000..d896ba6b9 --- /dev/null +++ b/src/interfaces/slice/jderobot/rgbd.ice @@ -0,0 +1,42 @@ +/* + * + * Copyright (C) 1997-2017 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/. + * + * Author: Francisco Rivas + * + */ + +#ifndef RGBD_ICE +#define RGBD_ICE + +#include + +module jderobot{ + struct rgbData { + ImageData color; + ImageData depth; + }; + + + interface rgbd{ + idempotent rgbData getData(); + + }; +}; + + + +#endif \ No newline at end of file diff --git a/src/libs/jderobotutil/CMakeLists.txt b/src/libs/jderobotutil/CMakeLists.txt index 37d2b8af0..3aa678035 100644 --- a/src/libs/jderobotutil/CMakeLists.txt +++ b/src/libs/jderobotutil/CMakeLists.txt @@ -9,12 +9,13 @@ SET(JDEROBOT_UTILS_SOURCES paramdict time uncopyable.h - CameraTask - CameraHandler - CameraUtils + interfaceHandlers/CameraTask + interfaceHandlers/CameraHandler + interfaceHandlers/RGBDHandler + utils/CameraUtils ) -add_library (jderobotutil STATIC ${JDEROBOT_UTILS_SOURCES}) +add_library (jderobotutil STATIC ${JDEROBOT_UTILS_SOURCES} ) add_library (jderobotutilshare SHARED ${JDEROBOT_UTILS_SOURCES}) diff --git a/src/libs/jderobotutil/CameraHandler.cpp b/src/libs/jderobotutil/interfaceHandlers/CameraHandler.cpp similarity index 95% rename from src/libs/jderobotutil/CameraHandler.cpp rename to src/libs/jderobotutil/interfaceHandlers/CameraHandler.cpp index 8d802722d..e325174bb 100644 --- a/src/libs/jderobotutil/CameraHandler.cpp +++ b/src/libs/jderobotutil/interfaceHandlers/CameraHandler.cpp @@ -5,7 +5,7 @@ * Author: frivas */ -#include +#include namespace jderobot { @@ -25,7 +25,7 @@ CameraHandler::CameraHandler(std::string propertyPrefix, Ice::CommunicatorPtr ic //fill imageDescription imageDescription->width = prop->getPropertyAsIntWithDefault(prefix+"ImageWidth",340); - imageDescription->height = prop->getPropertyAsIntWithDefault(prefix+"ImageHeight",280); + imageDescription->height = prop->getPropertyAsIntWithDefault(prefix+"ImageHeight",240); } diff --git a/src/libs/jderobotutil/CameraHandler.h b/src/libs/jderobotutil/interfaceHandlers/CameraHandler.h similarity index 100% rename from src/libs/jderobotutil/CameraHandler.h rename to src/libs/jderobotutil/interfaceHandlers/CameraHandler.h diff --git a/src/libs/jderobotutil/CameraTask.cpp b/src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp similarity index 88% rename from src/libs/jderobotutil/CameraTask.cpp rename to src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp index 73161f094..4b0acd2df 100644 --- a/src/libs/jderobotutil/CameraTask.cpp +++ b/src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp @@ -11,8 +11,7 @@ #include #include #include - - +#include namespace jderobot { @@ -105,29 +104,18 @@ void CameraTask::sendImage(jderobot::AMD_ImageProvider_getImageDataPtr cb, std:: LOG(ERROR) << "Error, image with FORMAT_RGB8_Z must have 3 channels"; return; } - unsigned long source_len = image.rows*image.cols*3; - unsigned long compress_len = compressBound(source_len); - reply->pixelData.resize(compress_len); - unsigned char* compress_buf = (unsigned char *) malloc(compress_len); - int r = compress((Bytef *) compress_buf, (uLongf *) &compress_len, (const Bytef *) &(image.data[0]), (uLong)source_len ); - if(r != Z_OK) { - LOG(ERROR) << "Compression Error"; - switch(r) { - case Z_MEM_ERROR: - LOG(ERROR) << "Compression Error: Not enough memory to compress"; - break; - case Z_BUF_ERROR: - LOG(ERROR) << "Compression Error: Target buffer too small."; - break; - case Z_STREAM_ERROR: - LOG(ERROR) << "Compression Error: Invalid compression level."; - break; - } + unsigned char* compress_buf; + unsigned long compress_len; + + + if(CameraUtils::compressImage(image,compress_buf,compress_len)) { + LOG(ERROR) << "Error compressing image"; } else { + reply->pixelData.resize(compress_len); reply->description->format = colorspaces::ImageRGB8::FORMAT_RGB8_Z.get()->name; memcpy(&(reply->pixelData[0]), &(compress_buf[0]), compress_len); diff --git a/src/libs/jderobotutil/CameraTask.h b/src/libs/jderobotutil/interfaceHandlers/CameraTask.h similarity index 100% rename from src/libs/jderobotutil/CameraTask.h rename to src/libs/jderobotutil/interfaceHandlers/CameraTask.h diff --git a/src/libs/jderobotutil/interfaceHandlers/RGBDHandler.cpp b/src/libs/jderobotutil/interfaceHandlers/RGBDHandler.cpp new file mode 100644 index 000000000..efa9f8770 --- /dev/null +++ b/src/libs/jderobotutil/interfaceHandlers/RGBDHandler.cpp @@ -0,0 +1,39 @@ +// +// Created by frivas on 30/04/17. +// + +#include "RGBDHandler.h" + +namespace jderobot { + + RGBDHandler::RGBDHandler(std::string propertyPrefix, Ice::CommunicatorPtr ic) { + + } + + + RGBDHandler::~RGBDHandler() { + + } + + void RGBDHandler::setData(const cv::Mat &colorImage, const cv::Mat &depthImage) { + this->mutex.lock(); + this->colorImage = colorImage.clone(); + this->depthImage = depthImage.clone(); + this->mutex.unlock(); + } + + rgbData RGBDHandler::getData() { + rgbData data; + this->mutex.lock(); + data.color->pixelData.resize(this->colorImage.rows*this->colorImage.cols * 3); + memcpy(&(data.color->pixelData[0]),(unsigned char *) this->colorImage.data, this->colorImage.rows*this->colorImage.cols * 3); + + data.depth->pixelData.resize(this->depthImage.rows*this->depthImage.cols * 3); + memcpy(&(data.depth->pixelData[0]),(unsigned char *) this->depthImage.data, this->depthImage.rows*this->depthImage.cols * 3); + this->mutex.unlock(); + return data; + + } + + +} \ No newline at end of file diff --git a/src/libs/jderobotutil/interfaceHandlers/RGBDHandler.h b/src/libs/jderobotutil/interfaceHandlers/RGBDHandler.h new file mode 100644 index 000000000..c0ea020bf --- /dev/null +++ b/src/libs/jderobotutil/interfaceHandlers/RGBDHandler.h @@ -0,0 +1,35 @@ +// +// Created by frivas on 30/04/17. +// + +#ifndef JDEROBOT_RGBDHANDLER_H +#define JDEROBOT_RGBDHANDLER_H + + +#include +#include +#include + +namespace jderobot { + + class RGBDHandler : virtual public jderobot::rgbd { + public: + RGBDHandler(std::string propertyPrefix, Ice::CommunicatorPtr ic); + + virtual ~RGBDHandler(); + + void setData(const cv::Mat &colorImage, const cv::Mat &depthImage); + virtual rgbData getData(); + + + private: + Ice::PropertiesPtr prop; + std::string prefix; + IceUtil::Mutex mutex; + cv::Mat colorImage; + cv::Mat depthImage; + }; +} + + +#endif //JDEROBOT_RGBDHANDLER_H diff --git a/src/libs/jderobotutil/CameraUtils.cpp b/src/libs/jderobotutil/utils/CameraUtils.cpp similarity index 87% rename from src/libs/jderobotutil/CameraUtils.cpp rename to src/libs/jderobotutil/utils/CameraUtils.cpp index e5812a9ec..130db258c 100644 --- a/src/libs/jderobotutil/CameraUtils.cpp +++ b/src/libs/jderobotutil/utils/CameraUtils.cpp @@ -153,3 +153,29 @@ std::string CameraUtils::negotiateDefaultFormat(jderobot::CameraPrx prx, const s LOG(INFO) << "Negotiated format " + format + " for camera " + prx->getCameraDescription()->name; return format; } + +bool CameraUtils::compressImage(const cv::Mat &image, unsigned char *compressed_data,unsigned long& compress_len) { + unsigned long source_len = image.rows*image.cols*3; + compress_len = compressBound(source_len); + compressed_data = (unsigned char *) malloc(compress_len); + + int r = compress((Bytef *) compressed_data, (uLongf *) &compress_len, (const Bytef *) &(image.data[0]), (uLong)source_len ); + + if(r != Z_OK) { + LOG(WARNING) << "Compression Error"; + switch(r) { + case Z_MEM_ERROR: + LOG(ERROR) << "Compression Error: Not enough memory to compress"; + break; + case Z_BUF_ERROR: + LOG(ERROR) << "Compression Error: Target buffer too small."; + break; + case Z_STREAM_ERROR: + LOG(ERROR) << "Compression Error: Invalid compression level."; + break; + } + return false; + } + return true; +} + diff --git a/src/libs/jderobotutil/CameraUtils.h b/src/libs/jderobotutil/utils/CameraUtils.h similarity index 79% rename from src/libs/jderobotutil/CameraUtils.h rename to src/libs/jderobotutil/utils/CameraUtils.h index af3a087a8..d02298c3d 100644 --- a/src/libs/jderobotutil/CameraUtils.h +++ b/src/libs/jderobotutil/utils/CameraUtils.h @@ -14,6 +14,7 @@ class CameraUtils { public: static cv::Mat getImageFromCameraProxy(jderobot::ImageDataPtr dataPtr); static std::string negotiateDefaultFormat(jderobot::CameraPrx prx,const std::string& definedFormat ); + static bool compressImage(const cv::Mat& image, unsigned char* compressed_data,unsigned long& compress_len); }; diff --git a/src/libs/parallelIce/cameraClient.cpp b/src/libs/parallelIce/cameraClient.cpp index dfb37028a..804cabbd3 100644 --- a/src/libs/parallelIce/cameraClient.cpp +++ b/src/libs/parallelIce/cameraClient.cpp @@ -21,7 +21,7 @@ #include "cameraClient.h" #include -#include +#include namespace jderobot { diff --git a/src/tools/replayer2/interfaces/ReplayerCamera.h b/src/tools/replayer2/interfaces/ReplayerCamera.h index 8596b5d99..f151e1872 100644 --- a/src/tools/replayer2/interfaces/ReplayerCamera.h +++ b/src/tools/replayer2/interfaces/ReplayerCamera.h @@ -6,8 +6,8 @@ #define JDEROBOT_REPLAYERCAMERA_H -#include -#include +#include +#include #include #include #include From 2785fda944d6caa30995f356ac3cee59283b76a4 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Sat, 13 May 2017 20:49:46 +0200 Subject: [PATCH 2/3] #761 Included rgb interface support to openniServer --- src/drivers/openniServer/CMakeLists.txt | 12 ++- .../OpenNiServerLib/ConcurrentDevice.cpp | 34 ++++-- .../OpenNiServerLib/ConcurrentDevice.h | 5 +- .../OpenNiServerLib/RGBDServer.cpp | 101 ++++++++++++++++++ .../openniServer/OpenNiServerLib/RGBDServer.h | 58 ++++++++++ src/drivers/openniServer/openniServer.cfg | 6 +- src/drivers/openniServer/openniServer.cpp | 15 +++ .../interfaceHandlers/CameraTask.cpp | 2 +- src/libs/jderobotutil/utils/CameraUtils.cpp | 19 +++- src/libs/jderobotutil/utils/CameraUtils.h | 3 +- src/tools/rgbdViewer/rgbdViewer.cfg | 4 +- 11 files changed, 240 insertions(+), 19 deletions(-) create mode 100644 src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp create mode 100644 src/drivers/openniServer/OpenNiServerLib/RGBDServer.h diff --git a/src/drivers/openniServer/CMakeLists.txt b/src/drivers/openniServer/CMakeLists.txt index d64e8a14b..d12315049 100644 --- a/src/drivers/openniServer/CMakeLists.txt +++ b/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 diff --git a/src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.cpp b/src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.cpp index 1ff89338a..abee9cd67 100644 --- a/src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.cpp +++ b/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(); +} + diff --git a/src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.h b/src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.h index 50ab42cb3..8df4e62aa 100644 --- a/src/drivers/openniServer/OpenNiServerLib/ConcurrentDevice.h +++ b/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& distances); virtual void run(); void stop(); diff --git a/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp b/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp new file mode 100644 index 000000000..0173f9581 --- /dev/null +++ b/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp @@ -0,0 +1,101 @@ +// +// Created by frivas on 9/05/17. +// + +#include "RGBDServer.h" +#include +#include + + +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 " <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; + } +} \ No newline at end of file diff --git a/src/drivers/openniServer/OpenNiServerLib/RGBDServer.h b/src/drivers/openniServer/OpenNiServerLib/RGBDServer.h new file mode 100644 index 000000000..733487be9 --- /dev/null +++ b/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 +#include "ConcurrentDevice.h" +#include + + +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 ReplyCloudPtr; + ReplyCloudPtr replyCloud; + std::string prefix; + jderobot::rgbData data; + IceUtil::ThreadControl control; + ConcurrentDevicePtr device; + + }; + +} + +#endif //JDEROBOT_RGBDSERVER_H diff --git a/src/drivers/openniServer/openniServer.cfg b/src/drivers/openniServer/openniServer.cfg index ee943f3ee..dd4a37c15 100644 --- a/src/drivers/openniServer/openniServer.cfg +++ b/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 diff --git a/src/drivers/openniServer/openniServer.cpp b/src/drivers/openniServer/openniServer.cpp index ed3f26d73..1e645ae1e 100644 --- a/src/drivers/openniServer/openniServer.cpp +++ b/src/drivers/openniServer/openniServer.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #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(); diff --git a/src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp b/src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp index 4b0acd2df..9f5e201a0 100644 --- a/src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp +++ b/src/libs/jderobotutil/interfaceHandlers/CameraTask.cpp @@ -110,7 +110,7 @@ void CameraTask::sendImage(jderobot::AMD_ImageProvider_getImageDataPtr cb, std:: unsigned long compress_len; - if(CameraUtils::compressImage(image,compress_buf,compress_len)) { + if(!CameraUtils::compressImage(image,&compress_buf,compress_len)) { LOG(ERROR) << "Error compressing image"; } else diff --git a/src/libs/jderobotutil/utils/CameraUtils.cpp b/src/libs/jderobotutil/utils/CameraUtils.cpp index 130db258c..f4ab141ce 100644 --- a/src/libs/jderobotutil/utils/CameraUtils.cpp +++ b/src/libs/jderobotutil/utils/CameraUtils.cpp @@ -154,12 +154,12 @@ std::string CameraUtils::negotiateDefaultFormat(jderobot::CameraPrx prx, const s return format; } -bool CameraUtils::compressImage(const cv::Mat &image, unsigned char *compressed_data,unsigned long& compress_len) { +bool CameraUtils::compressImage(const cv::Mat &image, unsigned char **compressed_data,unsigned long& compress_len) { unsigned long source_len = image.rows*image.cols*3; compress_len = compressBound(source_len); - compressed_data = (unsigned char *) malloc(compress_len); + *compressed_data = (unsigned char *) malloc(compress_len); - int r = compress((Bytef *) compressed_data, (uLongf *) &compress_len, (const Bytef *) &(image.data[0]), (uLong)source_len ); + int r = compress((Bytef *) (*compressed_data), (uLongf *) &compress_len, (const Bytef *) &(image.data[0]), (uLong)source_len ); if(r != Z_OK) { LOG(WARNING) << "Compression Error"; @@ -179,3 +179,16 @@ bool CameraUtils::compressImage(const cv::Mat &image, unsigned char *compressed_ return true; } +jderobot::ImageDataPtr CameraUtils::convert(const cv::Mat &image) { + + jderobot::ImageDataPtr reply=jderobot::ImageDataPtr(new jderobot::ImageData()); + reply->description = jderobot::ImageDescriptionPtr(new jderobot::ImageDescription()); + IceUtil::Time t = IceUtil::Time::now(); + reply->timeStamp.seconds = (long)t.toSeconds(); + reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000; + reply->description->format = colorspaces::ImageRGB8::FORMAT_RGB8.get()->name; + reply->pixelData.resize(image.rows*image.cols * image.channels()); + memcpy(&(reply->pixelData[0]),(unsigned char *) image.data, image.rows*image.cols * image.channels()); + return reply; +} + diff --git a/src/libs/jderobotutil/utils/CameraUtils.h b/src/libs/jderobotutil/utils/CameraUtils.h index d02298c3d..781f3c0fd 100644 --- a/src/libs/jderobotutil/utils/CameraUtils.h +++ b/src/libs/jderobotutil/utils/CameraUtils.h @@ -14,7 +14,8 @@ class CameraUtils { public: static cv::Mat getImageFromCameraProxy(jderobot::ImageDataPtr dataPtr); static std::string negotiateDefaultFormat(jderobot::CameraPrx prx,const std::string& definedFormat ); - static bool compressImage(const cv::Mat& image, unsigned char* compressed_data,unsigned long& compress_len); + static bool compressImage(const cv::Mat& image, unsigned char** compressed_data,unsigned long& compress_len); + static jderobot::ImageDataPtr convert(const cv::Mat& image); }; diff --git a/src/tools/rgbdViewer/rgbdViewer.cfg b/src/tools/rgbdViewer/rgbdViewer.cfg index e743018cf..a2d0e9693 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cfg +++ b/src/tools/rgbdViewer/rgbdViewer.cfg @@ -1,11 +1,11 @@ rgbdViewer.CameraRGBActive=1 #rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h 193.147.14.20 -p 9998 rgbdViewer.CameraRGB.Fps=10 -rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9998 +rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9999 rgbdViewer.CameraDEPTHActive=1 rgbdViewer.CameraDEPTH.Fps=10 #rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h 193.147.14.20 -p 9998 -rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9998 +rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9999 rgbdViewer.pointCloudActive=0 #rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h 193.147.14.20 -p 9998 rgbdViewer.pointCloud.Fps=10 From 39d84f23ff3a0d254a5cd3a1b4dbf132cd3a7689 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Sat, 13 May 2017 21:33:10 +0200 Subject: [PATCH 3/3] #761 Included depth interface to rgbdViewer --- .../OpenNiServerLib/RGBDServer.cpp | 2 + src/libs/jderobotutil/utils/CameraUtils.cpp | 2 + src/tools/rgbdViewer/rgbdViewer.cfg | 8 +- src/tools/rgbdViewer/rgbdViewer.cpp | 146 ++++++++++++------ src/tools/rgbdViewer/rgbdViewergui.cpp | 5 +- 5 files changed, 113 insertions(+), 50 deletions(-) diff --git a/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp b/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp index 0173f9581..85e692179 100644 --- a/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp +++ b/src/drivers/openniServer/OpenNiServerLib/RGBDServer.cpp @@ -5,6 +5,7 @@ #include "RGBDServer.h" #include #include +#include namespace openniServer { @@ -71,6 +72,7 @@ namespace openniServer { jderobot::rgbData temporalData; temporalData.color = CameraUtils::convert(rgb); temporalData.depth = CameraUtils::convert(depth); + this->stableData= temporalData; this->mutex.unlock(); diff --git a/src/libs/jderobotutil/utils/CameraUtils.cpp b/src/libs/jderobotutil/utils/CameraUtils.cpp index f4ab141ce..16279221b 100644 --- a/src/libs/jderobotutil/utils/CameraUtils.cpp +++ b/src/libs/jderobotutil/utils/CameraUtils.cpp @@ -187,6 +187,8 @@ jderobot::ImageDataPtr CameraUtils::convert(const cv::Mat &image) { reply->timeStamp.seconds = (long)t.toSeconds(); reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000; reply->description->format = colorspaces::ImageRGB8::FORMAT_RGB8.get()->name; + reply->description->width=image.size().width; + reply->description->height=image.size().height; reply->pixelData.resize(image.rows*image.cols * image.channels()); memcpy(&(reply->pixelData[0]),(unsigned char *) image.data, image.rows*image.cols * image.channels()); return reply; diff --git a/src/tools/rgbdViewer/rgbdViewer.cfg b/src/tools/rgbdViewer/rgbdViewer.cfg index a2d0e9693..460a11b58 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cfg +++ b/src/tools/rgbdViewer/rgbdViewer.cfg @@ -1,8 +1,12 @@ -rgbdViewer.CameraRGBActive=1 +rgbdViewer.CameraRGBActive=0 +rgbdViewer.CameraDEPTHActive=0 +rgbdViewer.RGBDActive=1 + +rgbdViewer.RGBD.Proxy=rgbd1:tcp -h localhost -p 9999 + #rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h 193.147.14.20 -p 9998 rgbdViewer.CameraRGB.Fps=10 rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9999 -rgbdViewer.CameraDEPTHActive=1 rgbdViewer.CameraDEPTH.Fps=10 #rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h 193.147.14.20 -p 9998 rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9999 diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index 4d1c24d92..704bca39a 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include "rgbdViewergui.h" #include "pthread.h" #include "parallelIce/cameraClient.h" @@ -40,9 +42,13 @@ rgbdViewer::rgbdViewergui* rgbdViewergui_ptx; -jderobot::cameraClient* camRGB=NULL; -jderobot::cameraClient* camDEPTH=NULL; -jderobot::pointcloudClient* pcClient=NULL; +jderobot::CameraClientPtr camRGB; +jderobot::CameraClientPtr camDEPTH; +jderobot::PointcloudClientPtr pcClient; +jderobot::rgbdPrx rgbClient; +bool cameraRGBDActive=false; + + int debug; @@ -57,11 +63,19 @@ void *gui_thread(void* arg){ lastIT=IceUtil::Time::now(); while(rgbdViewergui_ptx->isVisible() && ! rgbdViewergui_ptx->isClosed()){ - if (camRGB!=NULL) - camRGB->getImage(rgb); - if (camDEPTH!=NULL) - camDEPTH->getImage(depth); - if (pcClient!=NULL) + + if (cameraRGBDActive) { + auto data = rgbClient->getData(); + rgb = CameraUtils::getImageFromCameraProxy(data.color); + depth = CameraUtils::getImageFromCameraProxy(data.depth); + } + else { + if (camRGB) + camRGB->getImage(rgb); + if (camDEPTH) + camDEPTH->getImage(depth); + } + if (pcClient) pcClient->getData(cloud); @@ -118,7 +132,12 @@ int main(int argc, char** argv){ bool pointCloudSelected=false; - + bool cameraRGBActive=false; + bool cameraDepthActive=false; + + + + pthread_attr_init(&attr); pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE); @@ -133,33 +152,66 @@ int main(int argc, char** argv){ std::cerr <<"Error :" << msg << std::endl; return 1; } - if (prop->getPropertyAsIntWithDefault("rgbdViewer.CameraRGBActive",0)){ - camRGB = new jderobot::cameraClient(ic,"rgbdViewer.CameraRGB."); - if (camRGB != NULL){ - rgbCamSelected=true; - camRGB->start(); - create_gui=true; + cameraRGBActive=(bool)prop->getPropertyAsIntWithDefault("rgbdViewer.CameraRGBActive",0); + cameraDepthActive=(bool)prop->getPropertyAsIntWithDefault("rgbdViewer.CameraDEPTHActive",0); + cameraRGBDActive=(bool)prop->getPropertyAsIntWithDefault("rgbdViewer.RGBDActive",0); + + if (cameraRGBDActive && (cameraRGBActive || cameraDepthActive)){ + LOG(ERROR) << "RGBD and single cameras cannot be selected at the same time"; + return 2; + } + + + if (cameraRGBDActive) { + std::string prefix = "rgbdViewer.RGBD."; + try{ + auto base = ic->propertyToProxy(prefix+"Proxy"); + if (0==base){ + throw prefix + "Could not create proxy with RGBD"; + } + else { + rgbClient = jderobot::rgbdPrx::checkedCast(base); + if (0==rgbClient) + throw "Invalid " + prefix + ".Proxy"; + } + }catch (const Ice::Exception& ex) { + std::cerr << ex << std::endl; } - else{ - throw "rgbdViewer: failed to load RGB Camera"; + catch (const char* msg) { + std::cerr << msg << std::endl; + LOG(FATAL) << prefix + " Not camera provided"; } - + create_gui = true; + rgbCamSelected=true; + depthCamSelected=true; } - if (prop->getPropertyAsIntWithDefault("rgbdViewer.CameraDEPTHActive",0)){ - camDEPTH = new jderobot::cameraClient(ic,"rgbdViewer.CameraDEPTH."); - if (camDEPTH != NULL){ - depthCamSelected=true; - camDEPTH->start(); - create_gui=true; + else{ + if (cameraRGBActive) { + camRGB = jderobot::CameraClientPtr(new jderobot::cameraClient(ic, "rgbdViewer.CameraRGB.")); + if (camRGB != NULL) { + rgbCamSelected = true; + camRGB->start(); + create_gui = true; + } else { + throw "rgbdViewer: failed to load RGB Camera"; + } + } - else{ - throw "rgbdViewer: failed to load DEPTH Camera"; + if (cameraDepthActive) { + camDEPTH =jderobot::CameraClientPtr( new jderobot::cameraClient(ic, "rgbdViewer.CameraDEPTH.")); + if (camDEPTH != NULL) { + depthCamSelected = true; + camDEPTH->start(); + create_gui = true; + } else { + throw "rgbdViewer: failed to load DEPTH Camera"; + } } } if (prop->getPropertyAsIntWithDefault("rgbdViewer.pointCloudActive",0)){ - pcClient = new jderobot::pointcloudClient(ic,"rgbdViewer.pointCloud."); + pcClient = jderobot::PointcloudClientPtr(new jderobot::pointcloudClient(ic,"rgbdViewer.pointCloud.")); if (pcClient!= NULL){ pcClient->start(); pointCloudSelected=true; @@ -174,21 +226,30 @@ int main(int argc, char** argv){ cv::Size rgbSize(0,0); cv::Size depthSize(0,0); //rgb - if (rgbCamSelected){ - while (rgbSize == cv::Size(0,0)){ - cv::Mat temp; - camRGB->getImage(temp,true); - rgbSize=temp.size(); - + if (cameraRGBDActive) { + while ((rgbSize == cv::Size(0, 0)) && (depthSize == cv::Size(0, 0))) { + auto data = rgbClient->getData(); + rgbSize = CameraUtils::getImageFromCameraProxy(data.color).size(); + depthSize = CameraUtils::getImageFromCameraProxy(data.depth).size(); } } - //depth - if (depthCamSelected){ - while (depthSize == cv::Size(0,0)){ - cv::Mat temp; - camDEPTH->getImage(temp,true); - depthSize=temp.size(); + else { + if (rgbCamSelected) { + while (rgbSize == cv::Size(0, 0)) { + cv::Mat temp; + camRGB->getImage(temp, true); + rgbSize = temp.size(); + } + } + //depth + if (depthCamSelected) { + while (depthSize == cv::Size(0, 0)) { + cv::Mat temp; + camDEPTH->getImage(temp, true); + depthSize = temp.size(); + + } } } @@ -212,13 +273,6 @@ int main(int argc, char** argv){ pthread_join(threads[i], NULL); } - if (camRGB!=NULL) - delete camRGB; - if (camDEPTH!=NULL) - delete camDEPTH; - if (pcClient!=NULL) - delete pcClient; - std::cout << "final" << std::endl; if (ic) ic->destroy(); diff --git a/src/tools/rgbdViewer/rgbdViewergui.cpp b/src/tools/rgbdViewer/rgbdViewergui.cpp index 324ac18c4..740018ce8 100644 --- a/src/tools/rgbdViewer/rgbdViewergui.cpp +++ b/src/tools/rgbdViewer/rgbdViewergui.cpp @@ -25,6 +25,7 @@ #include #include #include +#include namespace rgbdViewer { rgbdViewergui::rgbdViewergui(bool rgb, bool depth,bool pointCloud , std::string path, std::string path_rgb, std::string path_ir, cv::Size sizeRGB, cv::Size sizeDEPTH, float cycle): gtkmain(0,0) { @@ -184,8 +185,8 @@ rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vectorm_distance.lock(); for (int x=0; x< layers[1].cols ; x++) {