From ca387328429c749c133fd069c645d836a0cb0cb6 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Sun, 21 May 2017 18:25:20 +0200 Subject: [PATCH 1/4] #809 Refactored ringBuffer with a template class --- src/tools/recorder2/CMakeLists.txt | 5 +- src/tools/recorder2/buffer/ImageRingNode.cpp | 27 ++ src/tools/recorder2/buffer/ImageRingNode.h | 25 ++ src/tools/recorder2/buffer/RingBuffer.cpp | 101 -------- src/tools/recorder2/buffer/RingBuffer.h | 87 +++++-- src/tools/recorder2/pools/PoolPaths.cpp | 4 + src/tools/recorder2/pools/PoolWriteImages.cpp | 4 +- src/tools/recorder2/pools/PoolWriteImages.h | 10 +- src/tools/recorder2/pools/PoolWriteRGBD.cpp | 235 ++++++++++++++++++ src/tools/recorder2/pools/PoolWriteRGBD.h | 55 ++++ src/tools/recorder2/pools/PoolsManager.h | 2 +- src/tools/recorder2/pools/RecorderPool.h | 8 + src/tools/recorder2/pools/RecorderRGBD.cpp | 23 ++ src/tools/recorder2/pools/RecorderRGBD.h | 25 ++ .../recorder2/pools/poolWritePointCloud.h | 2 +- src/tools/recorder2/recorder2.cpp | 2 +- src/tools/rgbdViewer/CMakeLists.txt | 5 +- 17 files changed, 487 insertions(+), 133 deletions(-) create mode 100644 src/tools/recorder2/buffer/ImageRingNode.cpp create mode 100644 src/tools/recorder2/buffer/ImageRingNode.h create mode 100644 src/tools/recorder2/pools/PoolWriteRGBD.cpp create mode 100644 src/tools/recorder2/pools/PoolWriteRGBD.h create mode 100644 src/tools/recorder2/pools/RecorderRGBD.cpp create mode 100644 src/tools/recorder2/pools/RecorderRGBD.h diff --git a/src/tools/recorder2/CMakeLists.txt b/src/tools/recorder2/CMakeLists.txt index d028e8f6b..282636f8a 100644 --- a/src/tools/recorder2/CMakeLists.txt +++ b/src/tools/recorder2/CMakeLists.txt @@ -12,7 +12,7 @@ SET(SOURCE_FILES pools/PoolsManager buffer/RecorderInterface pools/PoolPaths - ) + pools/PoolWriteRGBD.cpp pools/PoolWriteRGBD.h pools/RecorderRGBD.cpp pools/RecorderRGBD.h buffer/ImageRingNode.cpp buffer/ImageRingNode.h) add_definitions(-DGLADE_DIR="${gladedir}") @@ -53,7 +53,8 @@ TARGET_LINK_LIBRARIES(recorder2 JderobotInterfaces jderobotutil ns - ${resourcelocator_LIBRARIES} + ${ZLIB_LIBRARIES} + ${resourcelocator_LIBRARIES} ${GLOG_LIBRARIES} ) diff --git a/src/tools/recorder2/buffer/ImageRingNode.cpp b/src/tools/recorder2/buffer/ImageRingNode.cpp new file mode 100644 index 000000000..5de577c5e --- /dev/null +++ b/src/tools/recorder2/buffer/ImageRingNode.cpp @@ -0,0 +1,27 @@ +// +// Created by frivas on 21/05/17. + +#include +#include +#include "ImageRingNode.h" +#include + +namespace RingBufferNS { + void ImageRingNode::write(const std::string &nameLog, std::vector data2save) { + boost::posix_time::ptime init = boost::posix_time::microsec_clock::local_time(); + for (auto it = data2save.begin(); it < data2save.end(); it++) { + std::stringstream path; + path << "data-" << nameLog << "/images/camera" << it->cameraId << "/" << it->relativeTime << ".png"; + cv::imwrite(path.str(), it->frame, it->mCompression); + } + boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); + + boost::posix_time::time_duration total = end - init; + LOG(INFO) << "Total milliseconds spent: " << total.total_milliseconds() << " - " << "Total Size: " + << data2save.size() << std::endl; + + for (auto it = data2save.begin(); it < data2save.end(); it++) + it->frame.release(); + } + +} \ No newline at end of file diff --git a/src/tools/recorder2/buffer/ImageRingNode.h b/src/tools/recorder2/buffer/ImageRingNode.h new file mode 100644 index 000000000..b2ebe353c --- /dev/null +++ b/src/tools/recorder2/buffer/ImageRingNode.h @@ -0,0 +1,25 @@ +// +// Created by frivas on 21/05/17. +// + +#ifndef JDEROBOT_IMAGERINGNODE_H +#define JDEROBOT_IMAGERINGNODE_H + +#include + +namespace RingBufferNS{ + class ImageRingNode { + public: + static void write(const std::string& nameLog, std::vector data2save); + +// private: + long long int relativeTime; + cv::Mat frame; + int cameraId; + std::vector mCompression; + + + }; +} + +#endif //JDEROBOT_IMAGERINGNODE_H diff --git a/src/tools/recorder2/buffer/RingBuffer.cpp b/src/tools/recorder2/buffer/RingBuffer.cpp index fbdc7c88e..10991eceb 100644 --- a/src/tools/recorder2/buffer/RingBuffer.cpp +++ b/src/tools/recorder2/buffer/RingBuffer.cpp @@ -23,105 +23,4 @@ namespace recorder { - RingBuffer::RingBuffer(long int maxTime) - { - mMaxBufferTime = maxTime; - } - - RingBuffer::~RingBuffer() - { - - for (std::vector::iterator it = mBuffer.begin(); it < mBuffer.end(); it++ ) - { - it->frame.release(); - } - - mBuffer.clear(); - } - - bool RingBuffer::addNode(RingNode node) - { - - mBuffer.push_back(node); - return checkBuffer(); - } - - static void *write_thread(void* context){ - - ((RingBuffer *)context)->write_th(); - - pthread_exit(NULL); - return NULL; - } - - void RingBuffer::write_th() - { - boost::posix_time::ptime init = boost::posix_time::microsec_clock::local_time(); - for (std::vector::iterator it = mWriteBuffer.begin(); it < mWriteBuffer.end(); it++ ) - { - std::stringstream path; - path << "data-" << mNameLog << "/images/camera" << it->cameraId << "/" << it->relativeTime << ".png"; - cv::imwrite(path.str(), it->frame, mCompression); - } - boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); - - boost::posix_time::time_duration total = end - init; - std::cout << "Total milliseconds spent: " << total.total_milliseconds() << " - " << "Total Size: " << mBuffer.size() << std::endl; - - for (std::vector::iterator it = mWriteBuffer.begin(); it < mWriteBuffer.end(); it++ ) - it->frame.release(); - - - mWriteBuffer.clear(); - - } - - void RingBuffer::write(std::string nameLog, std::vector compression) - { - mCompression = compression; - mNameLog = nameLog; - - mWriteBuffer.resize(mBuffer.size()); - std::copy( mBuffer.begin(), mBuffer.end(), mWriteBuffer.begin() ); - - //std::cout << &(mBuffer[0].frame) << std::endl; - //std::cout << &(mWriteBuffer[0].frame) << std::endl; - - pthread_attr_init(&mAttr); - pthread_attr_setdetachstate(&mAttr, PTHREAD_CREATE_JOINABLE); - pthread_create(&mThread, &mAttr, write_thread, this); - - - } - - bool RingBuffer::checkBuffer() - { - - - for (std::vector::iterator it = mBuffer.begin(); it < mBuffer.end(); it++ ) - { - std::vector::iterator newer = mBuffer.end()-1; - //jderobot::Logger::getInstance()->info("Current: " + boost::lexical_cast(it->relativeTime) + - // " - Newer: " + boost::lexical_cast(newer->relativeTime)); - - - if ( ( newer->relativeTime - it->relativeTime) > mMaxBufferTime ) - { - it->frame.release(); - mBuffer.erase(it); - return true; - } - else - { - //jderobot::Logger::getInstance()->info("Older: " + boost::lexical_cast(mBuffer.begin()->relativeTime) + - // " - Newer: " + boost::lexical_cast((mBuffer.end()-1)->relativeTime)); - - return false; - } - } - - return true; - - } - } \ No newline at end of file diff --git a/src/tools/recorder2/buffer/RingBuffer.h b/src/tools/recorder2/buffer/RingBuffer.h index 026cd3720..e24f9fb7f 100644 --- a/src/tools/recorder2/buffer/RingBuffer.h +++ b/src/tools/recorder2/buffer/RingBuffer.h @@ -29,30 +29,81 @@ namespace recorder { - + template class RingBuffer { public: - - class RingNode - { - public: - long long int relativeTime; - cv::Mat frame; - int cameraId; + struct pthread_create_args{ + std::vector buffer; + std::string logName; }; - RingBuffer(long int maxTime); - ~RingBuffer(); - bool addNode(RingNode node); - void write(std::string nameLog, std::vector compression); + RingBuffer(long int maxTime){ + mMaxBufferTime = maxTime; + } + ~RingBuffer(){ + for (auto it = mBuffer.begin(); it < mBuffer.end(); it++ ) + { + it->frame.release(); + } + + mBuffer.clear(); + } + + bool addNode(RingNode node){ + mBuffer.push_back(node); + return checkBuffer(); + } + void write(std::string nameLog, std::vector compression){ + mCompression = compression; + mNameLog = nameLog; + + mWriteBuffer.resize(mBuffer.size()); + std::copy( mBuffer.begin(), mBuffer.end(), mWriteBuffer.begin() ); + + //std::cout << &(mBuffer[0].frame) << std::endl; + //std::cout << &(mWriteBuffer[0].frame) << std::endl; + + pthread_attr_init(&mAttr); + pthread_attr_setdetachstate(&mAttr, PTHREAD_CREATE_JOINABLE); + + pthread_create_args args; + args.buffer= this->mWriteBuffer; + args.logName=nameLog; + + pthread_create(&mThread, &mAttr, write_thread, &args); + } - void write_th(); private: - bool checkBuffer(); + bool checkBuffer(){ + + for (auto it = mBuffer.begin(); it < mBuffer.end(); it++ ) + { + auto newer = mBuffer.end()-1; + //jderobot::Logger::getInstance()->info("Current: " + boost::lexical_cast(it->relativeTime) + + // " - Newer: " + boost::lexical_cast(newer->relativeTime)); + + + if ( ( newer->relativeTime - it->relativeTime) > mMaxBufferTime ) + { + it->frame.release(); + mBuffer.erase(it); + return true; + } + else + { + //jderobot::Logger::getInstance()->info("Older: " + boost::lexical_cast(mBuffer.begin()->relativeTime) + + // " - Newer: " + boost::lexical_cast((mBuffer.end()-1)->relativeTime)); + + return false; + } + } + + return true; + } long int mMaxBufferTime; std::vector mBuffer; @@ -64,6 +115,14 @@ namespace recorder std::string mNameLog; + static void *write_thread(void* context){ + pthread_create_args *args = (struct pthread_create_args *)context; + + RingNode::write(args->logName, args->buffer); + pthread_exit(NULL); + return NULL; + } + }; diff --git a/src/tools/recorder2/pools/PoolPaths.cpp b/src/tools/recorder2/pools/PoolPaths.cpp index a0d357c92..02514597f 100644 --- a/src/tools/recorder2/pools/PoolPaths.cpp +++ b/src/tools/recorder2/pools/PoolPaths.cpp @@ -24,6 +24,8 @@ namespace recorder { this->typesSufix[ENCODERS] = "encoders"; this->typesSufix[POINTCLOUD] = "pointClouds"; this->typesSufix[IMAGES] = "images"; + this->typesSufix[RGBD] = "rgbd"; + this->typesSufixIndependent[LASERS] = "laser"; this->typesSufixIndependent[POSE3DENCODERS] = "pose3dencoder"; @@ -31,6 +33,8 @@ namespace recorder { this->typesSufixIndependent[ENCODERS] = "encoder"; this->typesSufixIndependent[POINTCLOUD] = "pointCloud"; this->typesSufixIndependent[IMAGES] = "camera"; + this->typesSufixIndependent[RGBD] = "rgbd"; + testPathAndCreateIfNotExists(this->baseLogPath); } diff --git a/src/tools/recorder2/pools/PoolWriteImages.cpp b/src/tools/recorder2/pools/PoolWriteImages.cpp index 7ee2bd661..bb7d93225 100644 --- a/src/tools/recorder2/pools/PoolWriteImages.cpp +++ b/src/tools/recorder2/pools/PoolWriteImages.cpp @@ -34,7 +34,7 @@ poolWriteImages::poolWriteImages(Ice::ObjectPrx prx, int freq, int poolSize, int if (mMode == SAVE_BUFFER) { LOG(INFO) << "Recorder run as buffer mode, with a buffer = " + boost::lexical_cast(mBufferSeconds) + " seconds."; - mBuffer = new RingBuffer(mBufferSeconds*1000); + mBuffer = new RingBuffer(mBufferSeconds*1000); } else { createDevicePath(IMAGES, cameraID); @@ -109,7 +109,7 @@ poolWriteImages::~poolWriteImages() { } - RingBuffer::RingNode node; + RingBufferNS::ImageRingNode node; node.cameraId = deviceID; node.relativeTime = relative; diff --git a/src/tools/recorder2/pools/PoolWriteImages.h b/src/tools/recorder2/pools/PoolWriteImages.h index 977b59e17..cfb7aa9c6 100644 --- a/src/tools/recorder2/pools/PoolWriteImages.h +++ b/src/tools/recorder2/pools/PoolWriteImages.h @@ -33,6 +33,7 @@ #include #include #include +#include #include "RecorderPool.h" #include "PoolPaths.h" @@ -42,13 +43,6 @@ namespace recorder{ class poolWriteImages: public RecorderPool, public PoolPaths { public: - enum MODE - { - WRITE_FRAME = 0, - SAVE_BUFFER, - WRITE_BUFFER, - WRITE_END_LOG - }; poolWriteImages(Ice::ObjectPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat, @@ -74,7 +68,7 @@ class poolWriteImages: public RecorderPool, public PoolPaths { jderobot::CameraPrx cameraPrx; // write log by demand - recorder::RingBuffer* mBuffer; + recorder::RingBuffer* mBuffer; pthread_mutex_t mModeMutex; std::string mNameLog; int mLastSecondsLog; diff --git a/src/tools/recorder2/pools/PoolWriteRGBD.cpp b/src/tools/recorder2/pools/PoolWriteRGBD.cpp new file mode 100644 index 000000000..7702445d1 --- /dev/null +++ b/src/tools/recorder2/pools/PoolWriteRGBD.cpp @@ -0,0 +1,235 @@ +/* +* Copyright (C) 1997-2017 JDERobot 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 2 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 Library General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * Author : Francisco Miguel Rivas Montero + * + */ + +#include "PoolWriteRGBD.h" +#include + + +namespace recorder { + + PoolWriteRGBD::PoolWriteRGBD(Ice::ObjectPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat, std::string fileFormat, + std::vector compression_params,const std::string& baseLogPath, MODE mode, int bufferSeconds, std::string videoMode): + RecorderPool(freq,poolSize,cameraID), + PoolPaths(baseLogPath), +// mBuffer(NULL), +// mLastSecondsLog(5), +// mBufferSeconds(bufferSeconds), + mMode(mode) + { +// this->cameraPrx = jderobot::CameraPrx::checkedCast(prx); +// if (0== this->cameraPrx) { +// LOG(ERROR) << "Invalid proxy"; +// } +// this->compression_params=compression_params; +// this->imageFormat=imageFormat; +// this->fileFormat=fileFormat; +// +// +// mNameLog = "alarm1"; +// mVideoMode = videoMode; +// if (mMode == SAVE_BUFFER) +// { +// LOG(INFO) << "Recorder run as buffer mode, with a buffer = " + boost::lexical_cast(mBufferSeconds) + " seconds."; +// mBuffer = new RingBuffer(mBufferSeconds*1000); +// } +// else { +// createDevicePath(IMAGES, cameraID); +// this->setLogFile(getDeviceLogFilePath(IMAGES, cameraID)); +// } +// mCamType = this->cameraPrx->getImageFormat().at(0); + } + + PoolWriteRGBD::~PoolWriteRGBD() { + + } + + void PoolWriteRGBD::consumer_thread() { + pthread_mutex_lock(&(this->mutex)); + if (this->data.size() > 0) { +// RecorderRGBDPtr data2Save; +// +// data2Save=RecorderRGBDPtr(new RecorderRGBD(this->data[0])); +// this->data.erase(this->data.begin()); +// +// long long int relative; +// relative = *(this->its.begin()); +// this->its.erase(this->its.begin()); +// pthread_mutex_unlock(&(this->mutex)); +// +// std::stringstream imageFileName;//create a stringstream +// imageFileName << getDeviceLogPath(IMAGES, this->deviceID) << relative << "." << fileFormat; +// +// MODE currentMode; +// pthread_mutex_lock(&(this->mModeMutex)); +// currentMode = mMode; +// pthread_mutex_unlock(&(this->mModeMutex)); +// +// // Normal mode +// if (currentMode == WRITE_FRAME) { +// +// cv::imwrite(imageFileName.str(), img2Save, this->compression_params); +// this->logfile << relative << std::endl; +// +// } else { +// // Save buffer in memory mode == SAVE_BUFFER +// cv::Mat saveImage; +// +// if (mVideoMode.compare("Video") == 0) { +// if (mCamType.compare("RGB8") == 0) { +// saveImage.create(img2Save.size(), CV_8UC3); +// cv::cvtColor(img2Save, saveImage, CV_BGR2RGB); +// } else if (mCamType.compare("DEPTH8_16") == 0) { +// saveImage.create(img2Save.size(), CV_8UC3); +// std::vector layers; +// cv::split(img2Save, layers); +// cv::cvtColor(layers[0], saveImage, CV_GRAY2RGB); +// +// } else { +// +// LOG(WARNING) << "mCamType not recognized " + mCamType; +// } +// } else if (mVideoMode.compare("Log") == 0) { +// saveImage.create(img2Save.size(), img2Save.type()); +// img2Save.copyTo(saveImage); +// } +// +// +// RingBuffer::RingNode node; +// node.cameraId = deviceID; +// node.relativeTime = relative; +// +// saveImage.copyTo(node.frame); +// mBuffer->addNode(node); +// +// if (currentMode == WRITE_BUFFER) { +// std::string fileLogPath = getCustomLogFilePath(IMAGES, this->deviceID, mNameLog); +// this->setLogFile(fileLogPath); +// +// +// LOG(INFO) << "Init recording log: " + mNameLog + " (camera" + +// boost::lexical_cast(this->deviceID) + " ) with " +// + boost::lexical_cast(mBufferSeconds) +// + " buffer seconds and " + boost::lexical_cast(mLastSecondsLog) + +// " at the end!"; +// +// mBuffer->write(mNameLog, this->compression_params); +// +// pthread_mutex_lock(&(this->mModeMutex)); +// mMode = WRITE_END_LOG; +// pthread_mutex_unlock(&(this->mModeMutex)); +// +// mFinalInit = boost::posix_time::second_clock::local_time(); +// +// } +// // Save the final seconds of recording and save 'data' file +// else if (currentMode == WRITE_END_LOG) { +// +// mFinalEnd = boost::posix_time::second_clock::local_time(); +// boost::posix_time::time_duration total = mFinalEnd - mFinalInit; +// +// if (total.seconds() > mLastSecondsLog) { +// std::vector res; +// std::string dataPath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); +// boost::filesystem::directory_iterator end_itr; +// for (boost::filesystem::directory_iterator itr(dataPath); itr != end_itr; ++itr) { +// if (itr->path().generic_string().find("png") == std::string::npos) +// continue; +// +// unsigned begin = itr->path().generic_string().find_last_of("/") + 1; +// unsigned end = itr->path().generic_string().find_last_of("."); +// if (begin == std::string::npos || end == std::string::npos) { +// LOG(WARNING) << "Error while parsed file " + itr->path().generic_string(); +// continue; +// } +// +// res.push_back(atoi(itr->path().generic_string().substr(begin, end - begin).c_str())); +// } +// +// std::sort(res.begin(), res.end()); +// for (std::vector::iterator it = res.begin(); it < res.end(); it++) { +// this->logfile << *it << std::endl; +// } +// +// this->logfile.close(); +// LOG(INFO) << "End recording log: " + mNameLog; +// +// // Save the videoa +// if (mVideoMode.compare("Video") == 0) { +// std::stringstream fileData, fileImage, command, command_video, fileVideo, tmpFileVideo; +// +// std::string basePath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); +// fileData << basePath << "cameraData.jde"; +// fileImage << basePath << "list.txt"; +// +// command << "cat " << fileData.str() << " | sed -e 's/$/.png/g' > " << fileImage.str(); +// +// system(command.str().c_str()); +// +// fileVideo << mNamePathVideo << "/" << mNameLog << "-" << this->deviceID << "-" << mCamType +// << ".avi"; +// tmpFileVideo << mNamePathVideo << "/" << mNameLog << "-" << this->deviceID << "-" +// << mCamType << ".mp4"; +// +// command_video << "cd " << basePath << ";"; +// command_video +// << "mencoder mf://@list.txt -mf w=320:h=240:fps=10:type=png -ovc lavc -lavcopts vcodec=mpeg4:mbd=2:trell -oac copy -o " +// << tmpFileVideo.str() << ";"; +// command_video << "ffmpeg -y -i " << tmpFileVideo.str() << " -vcodec libx264 " +// << fileVideo.str() << ";"; +// command_video << "rm -f " << tmpFileVideo.str() << ";"; +// command_video << "cd -; rm -rf " << "data-" << mNameLog; +// +// std::cout << command_video.str() << std::endl; +// +// system(command_video.str().c_str()); +// } +// +// pthread_mutex_lock(&(this->mModeMutex)); +// mMode = SAVE_BUFFER; +// pthread_mutex_unlock(&(this->mModeMutex)); +// } else { +// std::string basePath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); +// std::stringstream path; +// path << basePath << relative << "." << fileFormat; +// cv::imwrite(path.str(), saveImage, this->compression_params); +// } +// } +// } + + } else + pthread_mutex_unlock(&(this->mutex)); + usleep(1000); + + } + + void PoolWriteRGBD::producer_thread() { + + } + + void *PoolWriteRGBD::consumer_thread_imp() { + return nullptr; + } + + void *PoolWriteRGBD::producer_thread_imp() { + return nullptr; + } + +} \ No newline at end of file diff --git a/src/tools/recorder2/pools/PoolWriteRGBD.h b/src/tools/recorder2/pools/PoolWriteRGBD.h new file mode 100644 index 000000000..aeae43b80 --- /dev/null +++ b/src/tools/recorder2/pools/PoolWriteRGBD.h @@ -0,0 +1,55 @@ +/* +* Copyright (C) 1997-2017 JDERobot 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 2 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 Library General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * Author : Francisco Miguel Rivas Montero + * + */ + +#ifndef JDEROBOT_RGBDPOOL_H +#define JDEROBOT_RGBDPOOL_H + +#include +#include "RecorderPool.h" +#include "PoolPaths.h" +#include "RecorderRGBD.h" + +namespace recorder { + + class PoolWriteRGBD: public RecorderPool, public PoolPaths { + PoolWriteRGBD(Ice::ObjectPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat, + std::string fileFormat, std::vector compression_params, const std::string& baseLogPath, + MODE mode, int bufferSeconds, std::string videoMode); + virtual ~PoolWriteRGBD(); + void consumer_thread(); + void producer_thread(); + + virtual void* consumer_thread_imp(); + virtual void* producer_thread_imp(); + + + private: + jderobot::rgbdPrx rgbdPrx; + std::vector data; + MODE mMode; + + + }; + + typedef boost::shared_ptr PoolWriteRGBDPtr; +} + +#endif //JDEROBOT_RGBDPOOL_H diff --git a/src/tools/recorder2/pools/PoolsManager.h b/src/tools/recorder2/pools/PoolsManager.h index 9847e4371..13db9d8c6 100644 --- a/src/tools/recorder2/pools/PoolsManager.h +++ b/src/tools/recorder2/pools/PoolsManager.h @@ -8,7 +8,7 @@ #include "RecorderPool.h" namespace recorder { - enum RECORDER_POOL_TYPE{LASERS, POSE3DENCODERS,POSE3D,ENCODERS,POINTCLOUD,IMAGES}; + enum RECORDER_POOL_TYPE{LASERS, POSE3DENCODERS,POSE3D,ENCODERS,POINTCLOUD,IMAGES,RGBD}; class PoolsManager { public: PoolsManager(pthread_attr_t& attr, int nConsumers); diff --git a/src/tools/recorder2/pools/RecorderPool.h b/src/tools/recorder2/pools/RecorderPool.h index 9ef8fccd6..b5b17dfb2 100644 --- a/src/tools/recorder2/pools/RecorderPool.h +++ b/src/tools/recorder2/pools/RecorderPool.h @@ -14,6 +14,14 @@ namespace recorder { class RecorderPool; typedef boost::shared_ptr RecorderPoolPtr; + enum MODE + { + WRITE_FRAME = 0, + SAVE_BUFFER, + WRITE_BUFFER, + WRITE_END_LOG + }; + class RecorderPool { public: diff --git a/src/tools/recorder2/pools/RecorderRGBD.cpp b/src/tools/recorder2/pools/RecorderRGBD.cpp new file mode 100644 index 000000000..a81a2b20d --- /dev/null +++ b/src/tools/recorder2/pools/RecorderRGBD.cpp @@ -0,0 +1,23 @@ +// +// Created by frivas on 21/05/17. +// + +#include +#include "RecorderRGBD.h" + + +namespace recorder { + RecorderRGBD::RecorderRGBD(const jderobot::rgbData &data) { + rgb = CameraUtils::getImageFromCameraProxy(data.color); + depth = CameraUtils::getImageFromCameraProxy(data.depth); + } + + RecorderRGBD::RecorderRGBD(const RecorderRGBD &other) { + rgb= other.rgb.clone(); + depth = other.depth.clone(); + } + + RecorderRGBD::RecorderRGBD() { + + } +} \ No newline at end of file diff --git a/src/tools/recorder2/pools/RecorderRGBD.h b/src/tools/recorder2/pools/RecorderRGBD.h new file mode 100644 index 000000000..317a1370d --- /dev/null +++ b/src/tools/recorder2/pools/RecorderRGBD.h @@ -0,0 +1,25 @@ +// +// Created by frivas on 21/05/17. +// + +#ifndef JDEROBOT_RECORDERRGBD_H +#define JDEROBOT_RECORDERRGBD_H + + +#include +#include +#include + +namespace recorder{ + struct RecorderRGBD { + cv::Mat rgb; + cv::Mat depth; + RecorderRGBD(const jderobot::rgbData& data); + RecorderRGBD(const RecorderRGBD& other); + RecorderRGBD(); + }; + + typedef boost::shared_ptr RecorderRGBDPtr; +} + +#endif //JDEROBOT_RECORDERRGBD_H diff --git a/src/tools/recorder2/pools/poolWritePointCloud.h b/src/tools/recorder2/pools/poolWritePointCloud.h index 7313a8229..a22fa1d3f 100644 --- a/src/tools/recorder2/pools/poolWritePointCloud.h +++ b/src/tools/recorder2/pools/poolWritePointCloud.h @@ -1,5 +1,5 @@ /* -* Copyright (C) 1997-2013 JDERobot Developers Team +* Copyright (C) 1997-2017 JDERobot 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 diff --git a/src/tools/recorder2/recorder2.cpp b/src/tools/recorder2/recorder2.cpp index c95f87264..9276fc7ad 100644 --- a/src/tools/recorder2/recorder2.cpp +++ b/src/tools/recorder2/recorder2.cpp @@ -168,7 +168,7 @@ int main(int argc, char** argv){ sFormat << "Recorder.Camera" << i+1 << ".Format"; imageFormat = prop->getProperty(sFormat.str()); recorder::poolWriteImagesPtr temp = recorder::poolWriteImagesPtr( new recorder::poolWriteImages(cprxAux, Hz,poolSize,i+1, - imageFormat ,fileFormat ,compression_params,baseLogPath,(bufferEnabled == 0)? recorder::poolWriteImages::WRITE_FRAME : recorder::poolWriteImages::SAVE_BUFFER, bufferSeconds, videoMode)); + imageFormat ,fileFormat ,compression_params,baseLogPath,(bufferEnabled == 0)? recorder::WRITE_FRAME : recorder::SAVE_BUFFER, bufferSeconds, videoMode)); manager->addPool(recorder::IMAGES,temp); } diff --git a/src/tools/rgbdViewer/CMakeLists.txt b/src/tools/rgbdViewer/CMakeLists.txt index bcab1d5e9..fff30de87 100644 --- a/src/tools/rgbdViewer/CMakeLists.txt +++ b/src/tools/rgbdViewer/CMakeLists.txt @@ -40,9 +40,8 @@ TARGET_LINK_LIBRARIES(rgbdViewer ${ZeroCIce_LIBRARIES} ${ZLIB_LIBRARIES} ${resourcelocator_LIBRARIES} - ${GLOG_LIBRARIES} - - ) + ${GLOG_LIBRARIES} + ) install(TARGETS rgbdViewer DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ From 9d6270aa8c7ec098ebf734ae666c1e24fda7f0e9 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Sat, 27 May 2017 10:43:25 +0200 Subject: [PATCH 2/4] #809 Refactored ringBuffer with templates --- src/tools/recorder2/CMakeLists.txt | 2 +- src/tools/recorder2/buffer/ImageRingNode.cpp | 2 +- src/tools/recorder2/buffer/RGBDRingNode.cpp | 10 +++++++ src/tools/recorder2/buffer/RGBDRingNode.h | 29 +++++++++++++++++++ src/tools/recorder2/buffer/RingBuffer.h | 9 ++++-- src/tools/recorder2/pools/PoolWriteImages.cpp | 2 +- src/tools/recorder2/pools/PoolsManager.h | 2 +- src/tools/recorder2/recorder.cfg | 2 +- src/tools/recorder2/recorder/Common.cpp | 5 ++++ src/tools/recorder2/recorder/Common.h | 15 ++++++++++ 10 files changed, 70 insertions(+), 8 deletions(-) create mode 100644 src/tools/recorder2/buffer/RGBDRingNode.cpp create mode 100644 src/tools/recorder2/buffer/RGBDRingNode.h create mode 100644 src/tools/recorder2/recorder/Common.cpp create mode 100644 src/tools/recorder2/recorder/Common.h diff --git a/src/tools/recorder2/CMakeLists.txt b/src/tools/recorder2/CMakeLists.txt index 282636f8a..720f9266e 100644 --- a/src/tools/recorder2/CMakeLists.txt +++ b/src/tools/recorder2/CMakeLists.txt @@ -12,7 +12,7 @@ SET(SOURCE_FILES pools/PoolsManager buffer/RecorderInterface pools/PoolPaths - pools/PoolWriteRGBD.cpp pools/PoolWriteRGBD.h pools/RecorderRGBD.cpp pools/RecorderRGBD.h buffer/ImageRingNode.cpp buffer/ImageRingNode.h) + pools/PoolWriteRGBD.cpp pools/PoolWriteRGBD.h pools/RecorderRGBD.cpp pools/RecorderRGBD.h buffer/ImageRingNode.cpp buffer/ImageRingNode.h buffer/RGBDRingNode.cpp buffer/RGBDRingNode.h recorder/Common.cpp recorder/Common.h) add_definitions(-DGLADE_DIR="${gladedir}") diff --git a/src/tools/recorder2/buffer/ImageRingNode.cpp b/src/tools/recorder2/buffer/ImageRingNode.cpp index 5de577c5e..e7c203daa 100644 --- a/src/tools/recorder2/buffer/ImageRingNode.cpp +++ b/src/tools/recorder2/buffer/ImageRingNode.cpp @@ -11,7 +11,7 @@ namespace RingBufferNS { boost::posix_time::ptime init = boost::posix_time::microsec_clock::local_time(); for (auto it = data2save.begin(); it < data2save.end(); it++) { std::stringstream path; - path << "data-" << nameLog << "/images/camera" << it->cameraId << "/" << it->relativeTime << ".png"; + path << "pool-" << nameLog << "/" << it->relativeTime << ".png"; cv::imwrite(path.str(), it->frame, it->mCompression); } boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); diff --git a/src/tools/recorder2/buffer/RGBDRingNode.cpp b/src/tools/recorder2/buffer/RGBDRingNode.cpp new file mode 100644 index 000000000..fed796347 --- /dev/null +++ b/src/tools/recorder2/buffer/RGBDRingNode.cpp @@ -0,0 +1,10 @@ +// +// Created by frivas on 21/05/17. +// + +#include "RGBDRingNode.h" + +void RingBufferNS::RGBDRingNode::ImageRingNode::write(const std::string &nameLog, + std::vector data2save) { + +} diff --git a/src/tools/recorder2/buffer/RGBDRingNode.h b/src/tools/recorder2/buffer/RGBDRingNode.h new file mode 100644 index 000000000..7b05901ea --- /dev/null +++ b/src/tools/recorder2/buffer/RGBDRingNode.h @@ -0,0 +1,29 @@ +// +// Created by frivas on 21/05/17. +// + +#ifndef JDEROBOT_RGBDRINGNODE_H +#define JDEROBOT_RGBDRINGNODE_H + +#include + +namespace RingBufferNS { + + class RGBDRingNode { + class ImageRingNode { + public: + static void write(const std::string &nameLog, std::vector data2save); + +// private: + long long int relativeTime; + cv::Mat frame; + int cameraId; + std::vector mCompression; + + + }; + }; +} + + +#endif //JDEROBOT_RGBDRINGNODE_H diff --git a/src/tools/recorder2/buffer/RingBuffer.h b/src/tools/recorder2/buffer/RingBuffer.h index e24f9fb7f..3b74081e7 100644 --- a/src/tools/recorder2/buffer/RingBuffer.h +++ b/src/tools/recorder2/buffer/RingBuffer.h @@ -26,11 +26,12 @@ #include #include #include +#include namespace recorder { template - class RingBuffer + class RingBuffer: public PoolPaths { public: struct pthread_create_args{ @@ -39,8 +40,9 @@ namespace recorder }; - RingBuffer(long int maxTime){ + RingBuffer(long int maxTime,const std::string& rootLogPath, RECORDER_POOL_TYPE type):PoolPaths(rootLogPath){ mMaxBufferTime = maxTime; + type=type; } ~RingBuffer(){ for (auto it = mBuffer.begin(); it < mBuffer.end(); it++ ) @@ -70,7 +72,7 @@ namespace recorder pthread_create_args args; args.buffer= this->mWriteBuffer; - args.logName=nameLog; + args.logName= this->getDeviceLogPath(type,this->mWriteBuffer[0].cameraId); pthread_create(&mThread, &mAttr, write_thread, &args); } @@ -114,6 +116,7 @@ namespace recorder pthread_attr_t mAttr; std::string mNameLog; + RECORDER_POOL_TYPE type; static void *write_thread(void* context){ pthread_create_args *args = (struct pthread_create_args *)context; diff --git a/src/tools/recorder2/pools/PoolWriteImages.cpp b/src/tools/recorder2/pools/PoolWriteImages.cpp index bb7d93225..cc6cf31a3 100644 --- a/src/tools/recorder2/pools/PoolWriteImages.cpp +++ b/src/tools/recorder2/pools/PoolWriteImages.cpp @@ -34,7 +34,7 @@ poolWriteImages::poolWriteImages(Ice::ObjectPrx prx, int freq, int poolSize, int if (mMode == SAVE_BUFFER) { LOG(INFO) << "Recorder run as buffer mode, with a buffer = " + boost::lexical_cast(mBufferSeconds) + " seconds."; - mBuffer = new RingBuffer(mBufferSeconds*1000); + mBuffer = new RingBuffer(mBufferSeconds*1000,"./",IMAGES); //todo fix path } else { createDevicePath(IMAGES, cameraID); diff --git a/src/tools/recorder2/pools/PoolsManager.h b/src/tools/recorder2/pools/PoolsManager.h index 13db9d8c6..9b9795ca8 100644 --- a/src/tools/recorder2/pools/PoolsManager.h +++ b/src/tools/recorder2/pools/PoolsManager.h @@ -5,10 +5,10 @@ #ifndef JDEROBOT_POOLSMANAGER_H #define JDEROBOT_POOLSMANAGER_H +#include #include "RecorderPool.h" namespace recorder { - enum RECORDER_POOL_TYPE{LASERS, POSE3DENCODERS,POSE3D,ENCODERS,POINTCLOUD,IMAGES,RGBD}; class PoolsManager { public: PoolsManager(pthread_attr_t& attr, int nConsumers); diff --git a/src/tools/recorder2/recorder.cfg b/src/tools/recorder2/recorder.cfg index c30c19863..56852900f 100644 --- a/src/tools/recorder2/recorder.cfg +++ b/src/tools/recorder2/recorder.cfg @@ -43,7 +43,7 @@ Recorder.Hz=5 # Config to buffer mode -Recorder.Buffer.Enabled=0 +Recorder.Buffer.Enabled=1 Recorder.Buffer.Seconds=30 # Mode = Log or Video Recorder.Buffer.Mode=Video diff --git a/src/tools/recorder2/recorder/Common.cpp b/src/tools/recorder2/recorder/Common.cpp new file mode 100644 index 000000000..c90f4374f --- /dev/null +++ b/src/tools/recorder2/recorder/Common.cpp @@ -0,0 +1,5 @@ +// +// Created by frivas on 21/05/17. +// + +#include "Common.h" diff --git a/src/tools/recorder2/recorder/Common.h b/src/tools/recorder2/recorder/Common.h new file mode 100644 index 000000000..5ba53f3e2 --- /dev/null +++ b/src/tools/recorder2/recorder/Common.h @@ -0,0 +1,15 @@ +// +// Created by frivas on 21/05/17. +// + +#ifndef JDEROBOT_COMMON_H +#define JDEROBOT_COMMON_H + +namespace recorder { + enum RECORDER_POOL_TYPE { + LASERS, POSE3DENCODERS, POSE3D, ENCODERS, POINTCLOUD, IMAGES, RGBD + }; +} + + +#endif //JDEROBOT_COMMON_H From 0d91a8aec1ccd5f0d18555824b7a553986e40c85 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Tue, 30 May 2017 18:16:15 +0200 Subject: [PATCH 3/4] #809 Fnal fix of refactorization and tested working --- .../recorder2/buffer/RecorderInterface.cpp | 4 ++- .../recorder2/buffer/RecorderInterface.h | 2 +- src/tools/recorder2/buffer/RingBuffer.h | 29 +++++++++++++++---- src/tools/recorder2/pools/PoolWriteImages.cpp | 4 +++ 4 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/tools/recorder2/buffer/RecorderInterface.cpp b/src/tools/recorder2/buffer/RecorderInterface.cpp index 7922ecc43..15c00dea7 100644 --- a/src/tools/recorder2/buffer/RecorderInterface.cpp +++ b/src/tools/recorder2/buffer/RecorderInterface.cpp @@ -11,14 +11,16 @@ namespace recorder { RecorderInterface::RecorderInterface(std::vector &poolImages): poolImages(poolImages) { - } bool RecorderInterface::saveLog(const ::std::string &name, ::Ice::Int seconds, const ::Ice::Current &ic) { bool ret = true; + for (size_t i=0; i< poolImages.size(); i++) { + + poolWriteImagesPtr pool = boost::dynamic_pointer_cast (poolImages[i]); bool log = pool->startCustomLog(name, seconds); ret = ret && log; diff --git a/src/tools/recorder2/buffer/RecorderInterface.h b/src/tools/recorder2/buffer/RecorderInterface.h index dd24cf53e..fb208b9bd 100644 --- a/src/tools/recorder2/buffer/RecorderInterface.h +++ b/src/tools/recorder2/buffer/RecorderInterface.h @@ -20,7 +20,7 @@ namespace recorder { saveVideo(const ::std::string &path, const ::std::string &name, ::Ice::Int seconds, const ::Ice::Current &ic); private: - std::vector &poolImages; + std::vector poolImages; }; } diff --git a/src/tools/recorder2/buffer/RingBuffer.h b/src/tools/recorder2/buffer/RingBuffer.h index 3b74081e7..b74caa1be 100644 --- a/src/tools/recorder2/buffer/RingBuffer.h +++ b/src/tools/recorder2/buffer/RingBuffer.h @@ -34,11 +34,18 @@ namespace recorder class RingBuffer: public PoolPaths { public: - struct pthread_create_args{ + + struct kkStruct{ + int value; + }; + + struct Pthread_create_args{ std::vector buffer; std::string logName; }; + typedef boost::shared_ptr Pthread_create_argsPtr; + RingBuffer(long int maxTime,const std::string& rootLogPath, RECORDER_POOL_TYPE type):PoolPaths(rootLogPath){ mMaxBufferTime = maxTime; @@ -70,11 +77,15 @@ namespace recorder pthread_attr_init(&mAttr); pthread_attr_setdetachstate(&mAttr, PTHREAD_CREATE_JOINABLE); - pthread_create_args args; - args.buffer= this->mWriteBuffer; - args.logName= this->getDeviceLogPath(type,this->mWriteBuffer[0].cameraId); + Pthread_create_args *args= new Pthread_create_args(); + + std::cout << "writeBuffer: " << this->mWriteBuffer.size() << std::endl; + args->buffer.resize(this->mWriteBuffer.size()); + std::copy(this->mWriteBuffer.begin(),this->mWriteBuffer.end(), args->buffer.begin()); + args->buffer= this->mWriteBuffer; + args->logName=std::string(this->getDeviceLogPath(type,this->mWriteBuffer[0].cameraId).c_str()); - pthread_create(&mThread, &mAttr, write_thread, &args); + pthread_create(&mThread, &mAttr, write_thread, args); } @@ -119,9 +130,15 @@ namespace recorder RECORDER_POOL_TYPE type; static void *write_thread(void* context){ - pthread_create_args *args = (struct pthread_create_args *)context; + Pthread_create_args* args = (Pthread_create_args *)context; + std::cout << "name: " << args->logName << std::endl; + std::cout << "size: " << args->buffer.size() << std::endl; +// RingNode::write(args->logName, args->buffer); + + args->buffer.clear(); + delete args; pthread_exit(NULL); return NULL; } diff --git a/src/tools/recorder2/pools/PoolWriteImages.cpp b/src/tools/recorder2/pools/PoolWriteImages.cpp index cc6cf31a3..1ef240a33 100644 --- a/src/tools/recorder2/pools/PoolWriteImages.cpp +++ b/src/tools/recorder2/pools/PoolWriteImages.cpp @@ -48,6 +48,8 @@ poolWriteImages::~poolWriteImages() { } void poolWriteImages::consumer_thread(){ + +// std::cout << "consumer: " << this->images.size() << std::endl; pthread_mutex_lock(&(this->mutex)); if (this->images.size()>0){ cv::Mat img2Save; @@ -116,6 +118,7 @@ poolWriteImages::~poolWriteImages() { saveImage.copyTo(node.frame); mBuffer->addNode(node); + if (currentMode == WRITE_BUFFER) { std::string fileLogPath = getCustomLogFilePath(IMAGES,this->deviceID,mNameLog); @@ -139,6 +142,7 @@ poolWriteImages::~poolWriteImages() { else if (currentMode == WRITE_END_LOG) { + mFinalEnd = boost::posix_time::second_clock::local_time(); boost::posix_time::time_duration total = mFinalEnd - mFinalInit; From 562e51c8ad6bfa907a829e1598ea9a7afeb949d1 Mon Sep 17 00:00:00 2001 From: Francisco Rivas Date: Wed, 14 Jun 2017 21:27:55 +0200 Subject: [PATCH 4/4] #809 Included rgbd support for rgbd interface --- src/tools/recorder2/buffer/ImageRingNode.cpp | 18 +- src/tools/recorder2/buffer/ImageRingNode.h | 8 +- src/tools/recorder2/buffer/RGBDRingNode.cpp | 41 ++- src/tools/recorder2/buffer/RGBDRingNode.h | 21 +- .../recorder2/buffer/RecorderInterface.cpp | 17 +- .../recorder2/buffer/RecorderInterface.h | 8 +- src/tools/recorder2/buffer/RingBuffer.h | 22 +- src/tools/recorder2/pools/PoolWriteImages.cpp | 4 +- src/tools/recorder2/pools/PoolWriteImages.h | 4 +- src/tools/recorder2/pools/PoolWriteRGBD.cpp | 341 +++++++++++------- src/tools/recorder2/pools/PoolWriteRGBD.h | 24 ++ src/tools/recorder2/pools/RecorderPool.cpp | 2 + src/tools/recorder2/recorder.cfg | 19 +- src/tools/recorder2/recorder2.cpp | 77 ++-- src/tools/rgbdViewer/rgbdViewer.cfg | 6 +- 15 files changed, 400 insertions(+), 212 deletions(-) diff --git a/src/tools/recorder2/buffer/ImageRingNode.cpp b/src/tools/recorder2/buffer/ImageRingNode.cpp index e7c203daa..5eabf6556 100644 --- a/src/tools/recorder2/buffer/ImageRingNode.cpp +++ b/src/tools/recorder2/buffer/ImageRingNode.cpp @@ -7,13 +7,27 @@ #include namespace RingBufferNS { - void ImageRingNode::write(const std::string &nameLog, std::vector data2save) { + void ImageRingNode::write(const std::string& logPath, const std::string &nameLog, std::vector data2save) { boost::posix_time::ptime init = boost::posix_time::microsec_clock::local_time(); + std::stringstream root_path; + root_path << "data-" << nameLog << "/" << logPath << "/"; + std::string jdeFile = root_path.str() + "cameraData2.jde"; + std::ofstream myfile; + myfile.open (jdeFile); + + + + for (auto it = data2save.begin(); it < data2save.end(); it++) { std::stringstream path; - path << "pool-" << nameLog << "/" << it->relativeTime << ".png"; + path << root_path.str() << it->relativeTime << ".png"; + std::cout << "saving: " << path.str() << std::endl; cv::imwrite(path.str(), it->frame, it->mCompression); + myfile << it->relativeTime << "/n"; + + std::cout << it->relativeTime << " to: " << jdeFile << std::endl; } + myfile.close(); boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); boost::posix_time::time_duration total = end - init; diff --git a/src/tools/recorder2/buffer/ImageRingNode.h b/src/tools/recorder2/buffer/ImageRingNode.h index b2ebe353c..57a2cd0b6 100644 --- a/src/tools/recorder2/buffer/ImageRingNode.h +++ b/src/tools/recorder2/buffer/ImageRingNode.h @@ -8,17 +8,13 @@ #include namespace RingBufferNS{ - class ImageRingNode { - public: - static void write(const std::string& nameLog, std::vector data2save); + struct ImageRingNode { + static void write(const std::string& logPath, const std::string& nameLog, std::vector data2save); -// private: long long int relativeTime; cv::Mat frame; int cameraId; std::vector mCompression; - - }; } diff --git a/src/tools/recorder2/buffer/RGBDRingNode.cpp b/src/tools/recorder2/buffer/RGBDRingNode.cpp index fed796347..483717941 100644 --- a/src/tools/recorder2/buffer/RGBDRingNode.cpp +++ b/src/tools/recorder2/buffer/RGBDRingNode.cpp @@ -3,8 +3,45 @@ // #include "RGBDRingNode.h" +#include +#include +#include +#include -void RingBufferNS::RGBDRingNode::ImageRingNode::write(const std::string &nameLog, - std::vector data2save) { +namespace RingBufferNS { + + void RGBDRingNode::write(const std::string& logPath,const std::string &nameLog, std::vector data2save) { + boost::posix_time::ptime init = boost::posix_time::microsec_clock::local_time(); + std::string root_path; + root_path = "data-" + nameLog + "/" + logPath + "/"; + + boost::replace_all(root_path, "./", ""); + boost::replace_all(root_path, "//", "/"); + +// std::string jdeFile = root_path.str() + "cameraData2.jde"; +// std::ofstream myfile; +// myfile.open (jdeFile); + + for (auto it = data2save.begin(); it < data2save.end(); it++) { + std::stringstream pathRGB; + pathRGB << root_path << it->relativeTime << "rgb.png"; + cv::imwrite(pathRGB.str(), it->rgb, it->mCompression); + std::stringstream pathDEPTH; + pathDEPTH << root_path << it->relativeTime << "depth.png"; + cv::imwrite(pathDEPTH.str(), it->depth, it->mCompression); + } + + + boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time(); + + boost::posix_time::time_duration total = end - init; + LOG(INFO) << "Total milliseconds spent: " << total.total_milliseconds() << " - " << "Total Size: " + << data2save.size() << std::endl; + + for (auto it = data2save.begin(); it < data2save.end(); it++) { + it->depth.release(); + it->rgb.release(); + } + } } diff --git a/src/tools/recorder2/buffer/RGBDRingNode.h b/src/tools/recorder2/buffer/RGBDRingNode.h index 7b05901ea..b43507016 100644 --- a/src/tools/recorder2/buffer/RGBDRingNode.h +++ b/src/tools/recorder2/buffer/RGBDRingNode.h @@ -7,21 +7,16 @@ #include -namespace RingBufferNS { +namespace RingBufferNS{ + struct RGBDRingNode { + static void write(const std::string& logPath, const std::string& nameLog, std::vector data2save); - class RGBDRingNode { - class ImageRingNode { - public: - static void write(const std::string &nameLog, std::vector data2save); + long long int relativeTime; + cv::Mat depth; + cv::Mat rgb; -// private: - long long int relativeTime; - cv::Mat frame; - int cameraId; - std::vector mCompression; - - - }; + int cameraId; + std::vector mCompression; }; } diff --git a/src/tools/recorder2/buffer/RecorderInterface.cpp b/src/tools/recorder2/buffer/RecorderInterface.cpp index 15c00dea7..e8f02d2df 100644 --- a/src/tools/recorder2/buffer/RecorderInterface.cpp +++ b/src/tools/recorder2/buffer/RecorderInterface.cpp @@ -2,14 +2,16 @@ // Created by frivas on 3/04/17. // +#include +#include #include "RecorderInterface.h" namespace recorder { - RecorderInterface::RecorderInterface(std::vector &poolImages): - poolImages(poolImages) + RecorderInterface::RecorderInterface(PoolsManagerPtr& manager): + manager(manager) { } @@ -17,6 +19,9 @@ namespace recorder { { bool ret = true; + auto poolImages=manager->getPoolsByType(IMAGES); + auto poolRGBD=manager->getPoolsByType(RGBD); + for (size_t i=0; i< poolImages.size(); i++) { @@ -26,12 +31,20 @@ namespace recorder { ret = ret && log; } + for (size_t i=0; i< poolRGBD.size(); i++) + { + PoolWriteRGBDPtr pool = boost::dynamic_pointer_cast (poolRGBD[i]); + bool log = pool->startCustomLog(name, seconds); + ret = ret && log; + } + return ret; } bool RecorderInterface::saveVideo(const ::std::string &path, const ::std::string &name, ::Ice::Int seconds, const ::Ice::Current &ic) { bool ret = true; + auto poolImages=manager->getPoolsByType(IMAGES); for (size_t i = 0; i < poolImages.size(); i++) { RecorderPoolPtr test; poolWriteImagesPtr pool = boost::dynamic_pointer_cast (poolImages[i]); diff --git a/src/tools/recorder2/buffer/RecorderInterface.h b/src/tools/recorder2/buffer/RecorderInterface.h index fb208b9bd..53c0d6a72 100644 --- a/src/tools/recorder2/buffer/RecorderInterface.h +++ b/src/tools/recorder2/buffer/RecorderInterface.h @@ -7,12 +7,12 @@ #include -#include +#include namespace recorder { class RecorderInterface : virtual public jderobot::recorder { public: - RecorderInterface(std::vector &poolImages); + RecorderInterface(PoolsManagerPtr& manager); virtual bool saveLog(const ::std::string &name, ::Ice::Int seconds, const ::Ice::Current &ic); @@ -20,7 +20,9 @@ namespace recorder { saveVideo(const ::std::string &path, const ::std::string &name, ::Ice::Int seconds, const ::Ice::Current &ic); private: - std::vector poolImages; + PoolsManagerPtr manager; + + }; } diff --git a/src/tools/recorder2/buffer/RingBuffer.h b/src/tools/recorder2/buffer/RingBuffer.h index b74caa1be..583be7585 100644 --- a/src/tools/recorder2/buffer/RingBuffer.h +++ b/src/tools/recorder2/buffer/RingBuffer.h @@ -17,6 +17,13 @@ * Authors : Roberto Calvo Palomino * */ + + + +#ifndef JDEROBOT_RGINBUFFER__ +#define JDEROBOT_RGINBUFFER__ + + #include #include #include @@ -42,6 +49,7 @@ namespace recorder struct Pthread_create_args{ std::vector buffer; std::string logName; + std::string logPath; }; typedef boost::shared_ptr Pthread_create_argsPtr; @@ -49,7 +57,7 @@ namespace recorder RingBuffer(long int maxTime,const std::string& rootLogPath, RECORDER_POOL_TYPE type):PoolPaths(rootLogPath){ mMaxBufferTime = maxTime; - type=type; + this->type=type; } ~RingBuffer(){ for (auto it = mBuffer.begin(); it < mBuffer.end(); it++ ) @@ -83,7 +91,9 @@ namespace recorder args->buffer.resize(this->mWriteBuffer.size()); std::copy(this->mWriteBuffer.begin(),this->mWriteBuffer.end(), args->buffer.begin()); args->buffer= this->mWriteBuffer; - args->logName=std::string(this->getDeviceLogPath(type,this->mWriteBuffer[0].cameraId).c_str()); + args->logPath=std::string(this->getDeviceLogPath(type,this->mWriteBuffer[0].cameraId).c_str()); + args->logName=mNameLog; + pthread_create(&mThread, &mAttr, write_thread, args); } @@ -102,7 +112,6 @@ namespace recorder if ( ( newer->relativeTime - it->relativeTime) > mMaxBufferTime ) { - it->frame.release(); mBuffer.erase(it); return true; } @@ -133,9 +142,10 @@ namespace recorder Pthread_create_args* args = (Pthread_create_args *)context; std::cout << "name: " << args->logName << std::endl; + std::cout << "path: " << args->logPath << std::endl; std::cout << "size: " << args->buffer.size() << std::endl; // - RingNode::write(args->logName, args->buffer); + RingNode::write(args->logPath, args->logName, args->buffer); args->buffer.clear(); delete args; @@ -146,4 +156,6 @@ namespace recorder }; -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/src/tools/recorder2/pools/PoolWriteImages.cpp b/src/tools/recorder2/pools/PoolWriteImages.cpp index 1ef240a33..853f21c85 100644 --- a/src/tools/recorder2/pools/PoolWriteImages.cpp +++ b/src/tools/recorder2/pools/PoolWriteImages.cpp @@ -256,7 +256,7 @@ void poolWriteImages::producer_thread(){ gettimeofday(&lastTime,NULL); } - bool poolWriteImages::startCustomLog (std::string name, int seconds) + bool poolWriteImages::startCustomLog (const std::string& name, int seconds) { bool ret; @@ -280,7 +280,7 @@ void poolWriteImages::producer_thread(){ return ret; } - bool poolWriteImages::startCustomVideo(std::string path, std::string name, int seconds) + bool poolWriteImages::startCustomVideo(const std::string& path, std::string name, int seconds) { mNamePathVideo = path; return startCustomLog(name, seconds); diff --git a/src/tools/recorder2/pools/PoolWriteImages.h b/src/tools/recorder2/pools/PoolWriteImages.h index cfb7aa9c6..654e934cd 100644 --- a/src/tools/recorder2/pools/PoolWriteImages.h +++ b/src/tools/recorder2/pools/PoolWriteImages.h @@ -53,8 +53,8 @@ class poolWriteImages: public RecorderPool, public PoolPaths { void consumer_thread(); void producer_thread(); - bool startCustomLog(std::string name, int seconds); - bool startCustomVideo(std::string path, std::string name, int seconds); + bool startCustomLog(const std::string& name, int seconds); + bool startCustomVideo(const std::string& path, std::string name, int seconds); virtual void* consumer_thread_imp(); virtual void* producer_thread_imp(); diff --git a/src/tools/recorder2/pools/PoolWriteRGBD.cpp b/src/tools/recorder2/pools/PoolWriteRGBD.cpp index 7702445d1..1c1c4cb65 100644 --- a/src/tools/recorder2/pools/PoolWriteRGBD.cpp +++ b/src/tools/recorder2/pools/PoolWriteRGBD.cpp @@ -21,6 +21,7 @@ #include "PoolWriteRGBD.h" #include +#include namespace recorder { @@ -29,31 +30,32 @@ namespace recorder { std::vector compression_params,const std::string& baseLogPath, MODE mode, int bufferSeconds, std::string videoMode): RecorderPool(freq,poolSize,cameraID), PoolPaths(baseLogPath), -// mBuffer(NULL), -// mLastSecondsLog(5), -// mBufferSeconds(bufferSeconds), - mMode(mode) - { -// this->cameraPrx = jderobot::CameraPrx::checkedCast(prx); -// if (0== this->cameraPrx) { -// LOG(ERROR) << "Invalid proxy"; -// } -// this->compression_params=compression_params; -// this->imageFormat=imageFormat; -// this->fileFormat=fileFormat; -// -// -// mNameLog = "alarm1"; -// mVideoMode = videoMode; -// if (mMode == SAVE_BUFFER) -// { -// LOG(INFO) << "Recorder run as buffer mode, with a buffer = " + boost::lexical_cast(mBufferSeconds) + " seconds."; -// mBuffer = new RingBuffer(mBufferSeconds*1000); -// } -// else { -// createDevicePath(IMAGES, cameraID); -// this->setLogFile(getDeviceLogFilePath(IMAGES, cameraID)); -// } + mMode(mode), + mLastSecondsLog(5), + mBufferSeconds(bufferSeconds), + mBuffer(NULL), + fileFormat(fileFormat) + +{ + this->rgbdPrx = jderobot::rgbdPrx::checkedCast(prx); + if (0== this->rgbdPrx) { + LOG(ERROR) << "Invalid proxy to RGBD"; + } + this->compression_params=compression_params; + + + + mNameLog = "alarm1"; + mVideoMode = videoMode; + if (mMode == SAVE_BUFFER) + { + LOG(INFO) << "Recorder run as buffer mode, with a buffer = " + boost::lexical_cast(mBufferSeconds) + " seconds."; + mBuffer = new RingBuffer(mBufferSeconds*1000,"./",RGBD); + } + else { + createDevicePath(RGBD, cameraID); + this->setLogFile(getDeviceLogFilePath(RGBD, cameraID)); + } // mCamType = this->cameraPrx->getImageFormat().at(0); } @@ -64,34 +66,37 @@ namespace recorder { void PoolWriteRGBD::consumer_thread() { pthread_mutex_lock(&(this->mutex)); if (this->data.size() > 0) { -// RecorderRGBDPtr data2Save; -// -// data2Save=RecorderRGBDPtr(new RecorderRGBD(this->data[0])); -// this->data.erase(this->data.begin()); -// -// long long int relative; -// relative = *(this->its.begin()); -// this->its.erase(this->its.begin()); -// pthread_mutex_unlock(&(this->mutex)); -// -// std::stringstream imageFileName;//create a stringstream -// imageFileName << getDeviceLogPath(IMAGES, this->deviceID) << relative << "." << fileFormat; -// -// MODE currentMode; -// pthread_mutex_lock(&(this->mModeMutex)); -// currentMode = mMode; -// pthread_mutex_unlock(&(this->mModeMutex)); -// -// // Normal mode -// if (currentMode == WRITE_FRAME) { -// -// cv::imwrite(imageFileName.str(), img2Save, this->compression_params); -// this->logfile << relative << std::endl; -// -// } else { -// // Save buffer in memory mode == SAVE_BUFFER -// cv::Mat saveImage; -// + RecorderRGBDPtr data2Save; + + data2Save=RecorderRGBDPtr(new RecorderRGBD(*this->data[0])); + this->data.erase(this->data.begin()); + + long long int relative; + relative = *(this->its.begin()); + this->its.erase(this->its.begin()); + pthread_mutex_unlock(&(this->mutex)); + + std::stringstream imageFileName;//create a stringstream + imageFileName << getDeviceLogPath(RGBD, this->deviceID) << relative ; //<< "." << fileFormat; + + MODE currentMode; + pthread_mutex_lock(&(this->mModeMutex)); + currentMode = mMode; + pthread_mutex_unlock(&(this->mModeMutex)); + + // Normal mode + if (currentMode == WRITE_FRAME) { + + cv::imwrite(imageFileName.str() + "-depth." + fileFormat, data2Save->depth, this->compression_params); + cv::imwrite(imageFileName.str() + "-rgb." + fileFormat, data2Save->rgb, this->compression_params); + + this->logfile << relative << std::endl; + + } else { + RingBufferNS::RGBDRingNode node; + // Save buffer in memory mode == SAVE_BUFFER + cv::Mat saveImage; + // if (mVideoMode.compare("Video") == 0) { // if (mCamType.compare("RGB8") == 0) { // saveImage.create(img2Save.size(), CV_8UC3); @@ -106,76 +111,76 @@ namespace recorder { // // LOG(WARNING) << "mCamType not recognized " + mCamType; // } -// } else if (mVideoMode.compare("Log") == 0) { -// saveImage.create(img2Save.size(), img2Save.type()); -// img2Save.copyTo(saveImage); -// } -// -// -// RingBuffer::RingNode node; -// node.cameraId = deviceID; -// node.relativeTime = relative; -// -// saveImage.copyTo(node.frame); -// mBuffer->addNode(node); -// -// if (currentMode == WRITE_BUFFER) { -// std::string fileLogPath = getCustomLogFilePath(IMAGES, this->deviceID, mNameLog); -// this->setLogFile(fileLogPath); -// -// -// LOG(INFO) << "Init recording log: " + mNameLog + " (camera" + -// boost::lexical_cast(this->deviceID) + " ) with " -// + boost::lexical_cast(mBufferSeconds) -// + " buffer seconds and " + boost::lexical_cast(mLastSecondsLog) + -// " at the end!"; -// -// mBuffer->write(mNameLog, this->compression_params); -// -// pthread_mutex_lock(&(this->mModeMutex)); -// mMode = WRITE_END_LOG; -// pthread_mutex_unlock(&(this->mModeMutex)); -// -// mFinalInit = boost::posix_time::second_clock::local_time(); -// -// } -// // Save the final seconds of recording and save 'data' file -// else if (currentMode == WRITE_END_LOG) { -// -// mFinalEnd = boost::posix_time::second_clock::local_time(); -// boost::posix_time::time_duration total = mFinalEnd - mFinalInit; -// -// if (total.seconds() > mLastSecondsLog) { -// std::vector res; -// std::string dataPath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); -// boost::filesystem::directory_iterator end_itr; -// for (boost::filesystem::directory_iterator itr(dataPath); itr != end_itr; ++itr) { -// if (itr->path().generic_string().find("png") == std::string::npos) -// continue; -// -// unsigned begin = itr->path().generic_string().find_last_of("/") + 1; -// unsigned end = itr->path().generic_string().find_last_of("."); -// if (begin == std::string::npos || end == std::string::npos) { -// LOG(WARNING) << "Error while parsed file " + itr->path().generic_string(); -// continue; -// } -// -// res.push_back(atoi(itr->path().generic_string().substr(begin, end - begin).c_str())); -// } -// -// std::sort(res.begin(), res.end()); -// for (std::vector::iterator it = res.begin(); it < res.end(); it++) { -// this->logfile << *it << std::endl; -// } -// -// this->logfile.close(); -// LOG(INFO) << "End recording log: " + mNameLog; -// -// // Save the videoa +// } else + if (mVideoMode.compare("Log") == 0) { + node.rgb=data2Save->rgb.clone(); + node.depth=data2Save->depth.clone(); + } + + + + node.cameraId = deviceID; + node.relativeTime = relative; + + mBuffer->addNode(node); + + if (currentMode == WRITE_BUFFER) { + std::string fileLogPath = getCustomLogFilePath(RGBD, this->deviceID, mNameLog); + this->setLogFile(fileLogPath); + + + LOG(INFO) << "Init recording log: " + mNameLog + " (camera" + + boost::lexical_cast(this->deviceID) + " ) with " + + boost::lexical_cast(mBufferSeconds) + + " buffer seconds and " + boost::lexical_cast(mLastSecondsLog) + + " at the end!"; + + mBuffer->write(mNameLog, this->compression_params); + + pthread_mutex_lock(&(this->mModeMutex)); + mMode = WRITE_END_LOG; + pthread_mutex_unlock(&(this->mModeMutex)); + + mFinalInit = boost::posix_time::second_clock::local_time(); + + } + // Save the final seconds of recording and save 'data' file + else if (currentMode == WRITE_END_LOG) { + + mFinalEnd = boost::posix_time::second_clock::local_time(); + boost::posix_time::time_duration total = mFinalEnd - mFinalInit; + + if (total.seconds() > mLastSecondsLog) { + std::vector res; + std::string dataPath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); + boost::filesystem::directory_iterator end_itr; + for (boost::filesystem::directory_iterator itr(dataPath); itr != end_itr; ++itr) { + if (itr->path().generic_string().find("png") == std::string::npos) + continue; + + unsigned begin = itr->path().generic_string().find_last_of("/") + 1; + unsigned end = itr->path().generic_string().find_last_of("."); + if (begin == std::string::npos || end == std::string::npos) { + LOG(WARNING) << "Error while parsed file " + itr->path().generic_string(); + continue; + } + + res.push_back(atoi(itr->path().generic_string().substr(begin, end - begin).c_str())); + } + + std::sort(res.begin(), res.end()); + for (std::vector::iterator it = res.begin(); it < res.end(); it++) { + this->logfile << *it << std::endl; + } + + this->logfile.close(); + LOG(INFO) << "End recording log: " + mNameLog; + + // Save the videoa // if (mVideoMode.compare("Video") == 0) { // std::stringstream fileData, fileImage, command, command_video, fileVideo, tmpFileVideo; // -// std::string basePath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); +// std::string basePath = getCustomLogPath(RGBD, this->deviceID, mNameLog); // fileData << basePath << "cameraData.jde"; // fileImage << basePath << "list.txt"; // @@ -201,18 +206,19 @@ namespace recorder { // // system(command_video.str().c_str()); // } -// -// pthread_mutex_lock(&(this->mModeMutex)); -// mMode = SAVE_BUFFER; -// pthread_mutex_unlock(&(this->mModeMutex)); -// } else { -// std::string basePath = getCustomLogPath(IMAGES, this->deviceID, mNameLog); -// std::stringstream path; -// path << basePath << relative << "." << fileFormat; -// cv::imwrite(path.str(), saveImage, this->compression_params); -// } -// } -// } + + pthread_mutex_lock(&(this->mModeMutex)); + mMode = SAVE_BUFFER; + pthread_mutex_unlock(&(this->mModeMutex)); + } else { + std::string basePath = getCustomLogPath(RGBD, this->deviceID, mNameLog); + std::stringstream path; + path << basePath << relative; + cv::imwrite(path.str() + "-depth." + fileFormat, data2Save->depth, this->compression_params); + cv::imwrite(path.str() + "-rgb." + fileFormat, data2Save->rgb, this->compression_params); + } + } + } } else pthread_mutex_unlock(&(this->mutex)); @@ -221,15 +227,80 @@ namespace recorder { } void PoolWriteRGBD::producer_thread() { + jderobot::rgbData dataInput = this->rgbdPrx->getData(); + + + RecorderRGBDPtr dataPtr = RecorderRGBDPtr ( new RecorderRGBD(dataInput)); + struct timeval now; + gettimeofday(&now,NULL); + long long int relative; + relative=((now.tv_sec*1000000+now.tv_usec) - (syncInitialTime.tv_sec*1000000+syncInitialTime.tv_usec))/1000; + pthread_mutex_lock(&(this->mutex)); + while (this->data.size() > this->poolSize){ + pthread_mutex_unlock(&(this->mutex)); + usleep(100); + pthread_mutex_lock(&(this->mutex)); + } + this->data.push_back(dataPtr); + this->its.push_back(relative); + pthread_mutex_unlock(&(this->mutex)); + gettimeofday(&now,NULL); + + long long int totalNow=now.tv_sec*1000000+now.tv_usec; + long long int totalLast=lastTime.tv_sec*1000000+lastTime.tv_usec; + + float sleepTime =this->cycle - (totalNow-totalLast)/1000.; + + if(sleepTime < 0 ) + sleepTime = 0; + usleep(sleepTime*1000); + gettimeofday(&lastTime,NULL); } void *PoolWriteRGBD::consumer_thread_imp() { - return nullptr; - } + while (this->getActive()){ + if (this->getRecording()) + this->consumer_thread(); + else + usleep(1000); + } + + pthread_exit(NULL); + return NULL; } void *PoolWriteRGBD::producer_thread_imp() { - return nullptr; + + while (this->getActive()){ + if (this->getRecording()) + this->producer_thread(); + else + usleep(1000); + } + pthread_exit(NULL); } + + bool PoolWriteRGBD::startCustomLog (const std::string& name, int seconds) + { + bool ret; + + pthread_mutex_lock(&(this->mModeMutex)); + + if (mMode != SAVE_BUFFER) + { + LOG(WARNING) << "Can't handle recording '" + name + "' because there is a recording active!" ; + ret = false; + } + else + { + mNameLog = name; + mLastSecondsLog = seconds; + mMode = WRITE_BUFFER; + + ret = true; + } + pthread_mutex_unlock(&(this->mModeMutex)); + + return ret; } } \ No newline at end of file diff --git a/src/tools/recorder2/pools/PoolWriteRGBD.h b/src/tools/recorder2/pools/PoolWriteRGBD.h index aeae43b80..8be1e7f90 100644 --- a/src/tools/recorder2/pools/PoolWriteRGBD.h +++ b/src/tools/recorder2/pools/PoolWriteRGBD.h @@ -23,6 +23,8 @@ #define JDEROBOT_RGBDPOOL_H #include +#include +#include #include "RecorderPool.h" #include "PoolPaths.h" #include "RecorderRGBD.h" @@ -30,6 +32,7 @@ namespace recorder { class PoolWriteRGBD: public RecorderPool, public PoolPaths { + public: PoolWriteRGBD(Ice::ObjectPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat, std::string fileFormat, std::vector compression_params, const std::string& baseLogPath, MODE mode, int bufferSeconds, std::string videoMode); @@ -37,6 +40,9 @@ namespace recorder { void consumer_thread(); void producer_thread(); + bool startCustomLog(const std::string& name, int seconds); +// bool startCustomVideo(const std::string& path, std::string name, int seconds); + virtual void* consumer_thread_imp(); virtual void* producer_thread_imp(); @@ -45,6 +51,24 @@ namespace recorder { jderobot::rgbdPrx rgbdPrx; std::vector data; MODE mMode; + int mLastSecondsLog; + int mBufferSeconds; + recorder::RingBuffer* mBuffer; + pthread_mutex_t mModeMutex; + + + std::vector compression_params; + std::string mNameLog; + + std::string mVideoMode; + std::string fileFormat; + + + boost::posix_time::ptime mFinalInit, mFinalEnd; + + + + }; diff --git a/src/tools/recorder2/pools/RecorderPool.cpp b/src/tools/recorder2/pools/RecorderPool.cpp index 372547b47..bb03184ee 100644 --- a/src/tools/recorder2/pools/RecorderPool.cpp +++ b/src/tools/recorder2/pools/RecorderPool.cpp @@ -15,6 +15,8 @@ namespace recorder { this->recording=false; this->cycle = 1000.0/freq; gettimeofday(&lastTime,NULL); + pthread_mutex_init(&this->mutex,NULL); + } bool RecorderPool::getActive() { diff --git a/src/tools/recorder2/recorder.cfg b/src/tools/recorder2/recorder.cfg index 56852900f..31a9005a3 100644 --- a/src/tools/recorder2/recorder.cfg +++ b/src/tools/recorder2/recorder.cfg @@ -1,7 +1,10 @@ Recorder.FileName=datos.jde -Recorder.nCameras=2 -Recorder.nDethSensors=1 +Recorder.nCameras=0 +Recorder.nDethSensors=0 Recorder.nLasers=0 +Recorder.nRGBDSensors=1 + + Recorder.DepthSensor1.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9999 Recorder.DepthSensor2.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998 Recorder.Laser1.Proxy=Laser:tcp -h localhost -p 9996 @@ -12,6 +15,12 @@ Recorder.Camera1.Format=RGB8 Recorder.Camera2.Proxy=cameraB:default -h localhost -p 9999 Recorder.Camera2.Format=RGB8 +Recorder.RGBD1.Proxy=rgbd1:default -h localhost -p 9999 +Recorder.RGBD1.Format=RGB8 + + + + #Para kinect #Recorder.Camera1.Proxy=cameraRGB:tcp -h localhost -p 9998 #Recorder.Camera1.Format=RGB8 @@ -27,7 +36,7 @@ Recorder.nEncoders=0 Recorder.Encoders1.Proxy=Encoders:tcp -h localhost -p 9997 Recorder.nPose3d=0 Recorder.Pose3D1.Proxy= ImuPlugin:default -h localhost -p 9000 -Recorder.GUI=1 +Recorder.GUI=0 Recorder.nConsumers=1 Recorder.poolSize=10 @@ -46,10 +55,10 @@ Recorder.Hz=5 Recorder.Buffer.Enabled=1 Recorder.Buffer.Seconds=30 # Mode = Log or Video -Recorder.Buffer.Mode=Video +Recorder.Buffer.Mode=Log Recorder.Endpoints=default -h 0.0.0.0 -p 9992 Recorder.Name=Recorder-Log # Naming Service (only to buffer mode) -NamingService.Enabled=1 +NamingService.Enabled=0 NamingService.Proxy=NamingServiceJdeRobot:default -h 0.0.0.0 -p 10000 diff --git a/src/tools/recorder2/recorder2.cpp b/src/tools/recorder2/recorder2.cpp index 9276fc7ad..1e0b94dda 100644 --- a/src/tools/recorder2/recorder2.cpp +++ b/src/tools/recorder2/recorder2.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include "recordergui.h" #include "pools/PoolWriteImages.h" #include "pools/poolWritePose3dEncoders.h" @@ -121,26 +122,23 @@ int main(int argc, char** argv){ int nCameras = prop->getPropertyAsIntWithDefault("Recorder.nCameras",0); - if (nCameras > 0 ){ - fileFormat=prop->getProperty("Recorder.FileFormat"); - if (fileFormat.compare(std::string("png"))==0){ - pngCompressRatio=prop->getPropertyAsIntWithDefault("Recorder.PngCompression",3); - compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION); - compression_params.push_back(pngCompressRatio); - } - else if (fileFormat.compare(std::string("jpg"))==0){ - jpgQuality=prop->getPropertyAsIntWithDefault("Recorder.JpgQuality",95); - compression_params.push_back(CV_IMWRITE_JPEG_QUALITY); - compression_params.push_back(jpgQuality); - } - else{ - throw "File format is not valid"; - } - nConsumers=prop->getPropertyAsIntWithDefault("Recorder.nConsumers",2); - poolSize=prop->getPropertyAsIntWithDefault("Recorder.poolSize",10); - - + nConsumers=prop->getPropertyAsIntWithDefault("Recorder.nConsumers",2); + poolSize=prop->getPropertyAsIntWithDefault("Recorder.poolSize",10); + fileFormat=prop->getProperty("Recorder.FileFormat"); + if (fileFormat.compare(std::string("png"))==0){ + pngCompressRatio=prop->getPropertyAsIntWithDefault("Recorder.PngCompression",3); + compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION); + compression_params.push_back(pngCompressRatio); + } + else if (fileFormat.compare(std::string("jpg"))==0){ + jpgQuality=prop->getPropertyAsIntWithDefault("Recorder.JpgQuality",95); + compression_params.push_back(CV_IMWRITE_JPEG_QUALITY); + compression_params.push_back(jpgQuality); } + else{ + throw "File format is not valid"; + } + std::string baseLogPath="./data/"; manager= recorder::PoolsManagerPtr( new recorder::PoolsManager(attr,nConsumers)); @@ -183,8 +181,10 @@ int main(int argc, char** argv){ adapter =ic->createObjectAdapterWithEndpoints("Recorder", Endpoints); std::string name = prop->getProperty("Recorder.Name"); LOG(INFO) << "Creating Recorder: " + name; - auto imagesPool=manager->getPoolsByType(recorder::IMAGES); - recorder::RecorderInterface* recorder_prx = new recorder::RecorderInterface(imagesPool); + recorder::RecorderInterface* recorder_prx = new recorder::RecorderInterface(manager); + + + adapter->add(recorder_prx, ic->stringToIdentity(name)); adapter->activate(); @@ -268,17 +268,6 @@ int main(int argc, char** argv){ int nDepthSensors = prop->getPropertyAsIntWithDefault("Recorder.nDethSensors",0); for (int i=0; i< nDepthSensors; i++){ - struct stat buf; - std::stringstream claserPath; - claserPath << "./data/pointClouds/pointCloud" << i+1; - if( stat( claserPath.str().c_str(), &buf ) == -1 ) - { - std::stringstream instruction; - instruction << "mkdir " << claserPath.str(); - system(instruction.str().c_str()); - } - - std::stringstream sProxy; // Get driver camera sProxy << "Recorder.DepthSensor" << i+1 << ".Proxy"; @@ -291,6 +280,30 @@ int main(int argc, char** argv){ manager->addPool(recorder::POINTCLOUD,temp); } + + int nRGBDSensors = prop->getPropertyAsIntWithDefault("Recorder.nRGBDSensors",0); + for (int i=0; i< nRGBDSensors; i++){ + std::stringstream sProxy; + // Get driver camera + sProxy << "Recorder.RGBD" << i+1 << ".Proxy"; + + Ice::ObjectPrx base = ic->propertyToProxy(sProxy.str()); + if (0==base){ + throw "Could not create proxy with RGBD"; + } + + std::stringstream sFormat; + std::string imageFormat; + sFormat << "Recorder.RGBD" << i+1 << ".Format"; + imageFormat = prop->getProperty(sFormat.str()); + + recorder::PoolWriteRGBDPtr temp = recorder::PoolWriteRGBDPtr( new recorder::PoolWriteRGBD(base, Hz,poolSize,i+1, + imageFormat ,fileFormat ,compression_params,baseLogPath,(bufferEnabled == 0)? recorder::WRITE_FRAME : recorder::SAVE_BUFFER, bufferSeconds, videoMode)); + + + manager->addPool(recorder::RGBD,temp); + } + manager->createThreads(); diff --git a/src/tools/rgbdViewer/rgbdViewer.cfg b/src/tools/rgbdViewer/rgbdViewer.cfg index 460a11b58..57c218358 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cfg +++ b/src/tools/rgbdViewer/rgbdViewer.cfg @@ -1,6 +1,6 @@ -rgbdViewer.CameraRGBActive=0 -rgbdViewer.CameraDEPTHActive=0 -rgbdViewer.RGBDActive=1 +rgbdViewer.CameraRGBActive=1 +rgbdViewer.CameraDEPTHActive=1 +rgbdViewer.RGBDActive=0 rgbdViewer.RGBD.Proxy=rgbd1:tcp -h localhost -p 9999