diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f947ad..9409748 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,8 @@ add_executable(LoggerRealSense CameraInterface.h RealSenseInterface.cpp RealSenseInterface.h + Logger.cpp + Logger.h ) target_link_libraries(LoggerRealSense diff --git a/CameraInterface.h b/CameraInterface.h index 8bae8c9..419e572 100644 --- a/CameraInterface.h +++ b/CameraInterface.h @@ -13,12 +13,18 @@ class CameraInterface { public: + CameraInterface(int width,int height,int fps) + : width(width),height(height),fps(fps) + { + } + virtual ~CameraInterface() {} virtual bool ok() = 0; virtual std::string error() = 0; virtual float depthScale() = 0; + const int width,height,fps; static const int numBuffers = 10; std::atomic latestDepthIndex; std::pair,int64_t> frameBuffers[numBuffers]; diff --git a/Logger.cpp b/Logger.cpp new file mode 100644 index 0000000..e8bab58 --- /dev/null +++ b/Logger.cpp @@ -0,0 +1,90 @@ +#include "Logger.h" + +#include + +using std::cout; + +Logger::Logger(string outDir,std::shared_ptr cam) + : + doWrite(false), + outDir(outDir), + fileId(0), + writeThread(nullptr), + cam(cam) +{ +} + +Logger::~Logger() +{ + if(doWrite) + { + stopWriting(); + } +} + +void Logger::startWriting() +{ + cout << "Starting writing.\n"; + doWrite = true; + nFrames = 0; + + string fn = outDir + "/seq" + std::to_string(fileId++) + ".klg"; + file.open(fn,std::ios::binary); + if(!file.is_open()) + { + cout << "Could not open file: " << fn << "\n"; + doWrite = false; + return; + } + + file.write((char*)&nFrames,sizeof(int32_t)); + writeThread = new std::thread(&Logger::write,this); +} + +void Logger::stopWriting() +{ + cout << "Stopping writing.\n"; + doWrite = false; + writeThread->join(); + delete writeThread; + writeThread = nullptr; + + file.seekp(0); + file.write((char*)&nFrames,sizeof(int32_t)); + + file.close(); + file.clear(); +} + +void Logger::write() +{ + static int lastWrittenBufferIdx = -1; + while(doWrite) + { + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + + int lastDepth = cam->latestDepthIndex; + if(lastDepth == -1) + continue; + + int bufferIdx = lastDepth % CameraInterface::numBuffers; + if(bufferIdx == lastWrittenBufferIdx) + continue; + + const void *depthData = cam->frameBuffers[bufferIdx].first.first; + const void *rgbData = cam->frameBuffers[bufferIdx].first.second; + + int64_t currTimestamp = cam->frameBuffers[bufferIdx].second; + int32_t depthSize = cam->width * cam->height * sizeof(uint16_t); + int32_t rgbSize = cam->width * cam->height * 3 * sizeof(uint8_t); + + file.write((char*)&currTimestamp,sizeof(int64_t)); + file.write((char*)&depthSize,sizeof(int32_t)); + file.write((char*)&rgbSize,sizeof(int32_t)); + file.write((char*)depthData,depthSize); + file.write((char*)rgbData,rgbSize); + + lastWrittenBufferIdx = bufferIdx; + nFrames++; + } +} \ No newline at end of file diff --git a/Logger.h b/Logger.h new file mode 100644 index 0000000..7aa5e6a --- /dev/null +++ b/Logger.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "CameraInterface.h" + +using std::string; + +/** +* Format is: +* int32_t at file beginning for frame count +* +* For each frame: +* int64_t: timestamp +* int32_t: depthSize +* int32_t: imageSize +* depthSize * unsigned char: depth_compress_buf +* imageSize * unsigned char: encodedImage->data.ptr +*/ + +class Logger +{ +public: + Logger(string outDir,std::shared_ptr cam); + ~Logger(); + + bool isWriting() const + { + return doWrite; + } + + void startWriting(); + void stopWriting(); + +private: + void write(); + + std::atomic doWrite; + string outDir; + int32_t nFrames; + int fileId; + std::ofstream file; + std::thread *writeThread; + std::shared_ptr cam; +}; diff --git a/README.md b/README.md index 224244d..3ab0208 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ +How to run: +Press button R to start recording and press it again to stop. + Dependencies: * librealsense diff --git a/RealSenseInterface.cpp b/RealSenseInterface.cpp index 0e14729..5be9bc9 100644 --- a/RealSenseInterface.cpp +++ b/RealSenseInterface.cpp @@ -2,9 +2,8 @@ #include RealSenseInterface::RealSenseInterface(int inWidth,int inHeight,int inFps) - : width(inWidth), - height(inHeight), - fps(inFps), + : + CameraInterface(inWidth,inHeight,inFps), dev(nullptr), initSuccessful(true) { diff --git a/RealSenseInterface.h b/RealSenseInterface.h index 0d7abcc..d32563c 100644 --- a/RealSenseInterface.h +++ b/RealSenseInterface.h @@ -16,8 +16,6 @@ class RealSenseInterface : public CameraInterface RealSenseInterface(int width = 640,int height = 480,int fps = 30); virtual ~RealSenseInterface(); - const int width,height,fps; - virtual bool ok() { return initSuccessful; diff --git a/main.cpp b/main.cpp index cc6cb28..32d1bd4 100644 --- a/main.cpp +++ b/main.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include "GL/glut.h" @@ -9,14 +9,16 @@ #include "CameraInterface.h" #include "RealSenseInterface.h" +#include "Logger.h" using std::cout; -using std::vector; +using std::string; const static int width = 640; const static int height = 480; const static int fps = 30; -std::unique_ptr cam; +std::shared_ptr cam; +std::unique_ptr logger; void displayCallback() { @@ -52,6 +54,10 @@ void idleCallback() void endApp() { glutLeaveMainLoop(); + if(logger->isWriting()) + { + logger->stopWriting(); + } } void keyboardCallback(unsigned char keyPressed,int mouseX,int mouseY) @@ -61,11 +67,12 @@ void keyboardCallback(unsigned char keyPressed,int mouseX,int mouseY) case 27: endApp(); break; - /*case 'r': - if(!lastPressStateKeyR) - restartGame(); - lastPressStateKeyR = true; - break;*/ + case 'r': + if(logger->isWriting()) + logger->stopWriting(); + else + logger->startWriting(); + break; default: break; } @@ -73,7 +80,12 @@ void keyboardCallback(unsigned char keyPressed,int mouseX,int mouseY) int main(int argc,char *argv[]) { - cam = std::make_unique(width,height,fps); + string outDir = ""; + if(argc > 1) + outDir = argv[1]; + + cam = std::make_shared(width,height,fps); + logger = std::make_unique(outDir,cam); glutInit(&argc,argv);