[Jderobot-admin] jderobot-r1232 - trunk/src/stable/components/recorder

rocapal en jderobot.org rocapal en jderobot.org
Mar Mayo 27 19:15:23 CEST 2014


Author: rocapal
Date: 2014-05-27 19:15:22 +0200 (Tue, 27 May 2014)
New Revision: 1232

Added:
   trunk/src/stable/components/recorder/RingBuffer.cpp
   trunk/src/stable/components/recorder/RingBuffer.h
Modified:
   trunk/src/stable/components/recorder/CMakeLists.txt
   trunk/src/stable/components/recorder/poolWriteImages.cpp
   trunk/src/stable/components/recorder/poolWriteImages.h
   trunk/src/stable/components/recorder/recorder.cfg
   trunk/src/stable/components/recorder/recorder.cpp
Log:
#249 Added buffer recording mode


Modified: trunk/src/stable/components/recorder/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/recorder/CMakeLists.txt	2014-05-27 17:02:31 UTC (rev 1231)
+++ trunk/src/stable/components/recorder/CMakeLists.txt	2014-05-27 17:15:22 UTC (rev 1232)
@@ -1,4 +1,4 @@
-SET(SOURCE_FILES recorder.cpp recordergui.cpp poolWriteImages.cpp poolWritePose3dEncoders.cpp poolWriteLasers.cpp poolWritePointCloud.cpp poolWriteEncoders.cpp)
+SET(SOURCE_FILES recorder.cpp recordergui.cpp poolWriteImages.cpp poolWritePose3dEncoders.cpp poolWriteLasers.cpp poolWritePointCloud.cpp poolWriteEncoders.cpp RingBuffer.h RingBuffer.cpp)
 
 add_definitions(-DGLADE_DIR="${gladedir}")
 
@@ -31,4 +31,5 @@
     colorspacesmm
     JderobotInterfaces
     jderobotutil
+    logger
 )

Added: trunk/src/stable/components/recorder/RingBuffer.cpp
===================================================================
--- trunk/src/stable/components/recorder/RingBuffer.cpp	                        (rev 0)
+++ trunk/src/stable/components/recorder/RingBuffer.cpp	2014-05-27 17:15:22 UTC (rev 1232)
@@ -0,0 +1,118 @@
+/*
+ *  Copyright (C) 2014 JdeRobot developers
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/.
+ *
+ *  Authors : Roberto Calvo Palomino <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include "RingBuffer.h"
+
+namespace recorder
+{
+
+RingBuffer::RingBuffer(long int maxTime)
+{
+	mMaxBufferTime = maxTime;
+}
+
+RingBuffer::~RingBuffer()
+{
+
+	for (std::vector<RingNode>::iterator it = mBuffer.begin(); it < mBuffer.end(); it++ )
+	{
+		it->frame.release();
+	}
+
+	mBuffer.clear();
+}
+
+bool RingBuffer::addNode(RingNode node)
+{
+
+	mBuffer.push_back(node);
+	//jderobot::Logger::getInstance()->info("Buffer Size: " + boost::lexical_cast<std::string>(mBuffer.size()));
+
+	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<RingNode>::iterator it = mBuffer.begin(); it < mBuffer.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() << std::endl;
+	std::cout << "Total Size: " << mBuffer.size() << std::endl;
+
+}
+
+void RingBuffer::write(std::string nameLog, std::vector<int> compression)
+{
+	mCompression = compression;
+	mNameLog = nameLog;
+
+	pthread_attr_init(&mAttr);
+	pthread_attr_setdetachstate(&mAttr, PTHREAD_CREATE_JOINABLE);
+	pthread_create(&mThread, &mAttr, write_thread, this);
+
+
+}
+
+bool RingBuffer::checkBuffer()
+{
+
+
+	for (std::vector<RingNode>::iterator it = mBuffer.begin(); it < mBuffer.end(); it++ )
+	{
+		std::vector<RingNode>::iterator newer = mBuffer.end()-1;
+		//jderobot::Logger::getInstance()->info("Current: " + boost::lexical_cast<std::string>(it->relativeTime) +
+		//		"  -  Newer: " + boost::lexical_cast<std::string>(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<std::string>(mBuffer.begin()->relativeTime) +
+			//			"  -  Newer: " + boost::lexical_cast<std::string>((mBuffer.end()-1)->relativeTime));
+
+			return false;
+		}
+	}
+
+	return true;
+
+}
+
+}

Added: trunk/src/stable/components/recorder/RingBuffer.h
===================================================================
--- trunk/src/stable/components/recorder/RingBuffer.h	                        (rev 0)
+++ trunk/src/stable/components/recorder/RingBuffer.h	2014-05-27 17:15:22 UTC (rev 1232)
@@ -0,0 +1,69 @@
+/*
+ *  Copyright (C) 2014 JdeRobot developers
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/.
+ *
+ *  Authors : Roberto Calvo Palomino <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+#include <boost/thread/thread.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <stdio.h>
+#include <log/Logger.h>
+#include <boost/lexical_cast.hpp>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+namespace recorder
+{
+
+class RingBuffer
+{
+	public:
+
+		class RingNode
+		{
+		public:
+			long long int relativeTime;
+			cv::Mat frame;
+			int cameraId;
+		};
+
+		RingBuffer(long int maxTime);
+		~RingBuffer();
+
+		bool addNode(RingNode node);
+		void write(std::string nameLog, std::vector<int> compression);
+
+		void write_th();
+
+	private:
+
+		bool checkBuffer();
+
+		long int mMaxBufferTime;
+		std::vector<RingNode> mBuffer;
+		std::vector<int> mCompression;
+
+		pthread_t mThread;
+		pthread_attr_t mAttr;
+
+		std::string mNameLog;
+
+};
+
+
+}

Modified: trunk/src/stable/components/recorder/poolWriteImages.cpp
===================================================================
--- trunk/src/stable/components/recorder/poolWriteImages.cpp	2014-05-27 17:02:31 UTC (rev 1231)
+++ trunk/src/stable/components/recorder/poolWriteImages.cpp	2014-05-27 17:15:22 UTC (rev 1232)
@@ -6,11 +6,14 @@
  */
 
 #include "poolWriteImages.h"
+#include <libgen.h>
 
 namespace recorder{
-poolWriteImages::poolWriteImages(jderobot::CameraPrx prx, int freq, int poolSize, int cameraID,  std::string imageFormat, std::vector<int> compression_params) {
+poolWriteImages::poolWriteImages(jderobot::CameraPrx prx, int freq, int poolSize, int cameraID,  std::string imageFormat, std::vector<int> compression_params, MODE mode, int bufferSeconds) {
 	// TODO Auto-generated constructor stub
 	pthread_mutex_init(&(this->mutex), NULL);
+	pthread_mutex_init(&(this->mModeMutex), NULL);
+
 	this->poolSize=poolSize;
 	this->compression_params=compression_params;
 	this->cameraID=cameraID;
@@ -18,6 +21,20 @@
 	this->imageFormat=imageFormat;
 	this->prx=prx;
 	this->freq=freq;
+
+	mBufferSeconds = bufferSeconds;
+	mMode = mode;
+	mBuffer = NULL;
+	mLastSecondsLog = 5;
+	mNameLog = "alarm1";
+
+	if (mMode == SAVE_BUFFER)
+	{
+		jderobot::Logger::getInstance()->info("Recorder run as buffer mode, with a buffer = " + boost::lexical_cast<std::string>(mBufferSeconds) + " seconds.");
+		mBuffer = new RingBuffer(mBufferSeconds*1000);
+	}
+
+
 	std::stringstream filePath;
 	filePath << "data/images/camera" << this->cameraID << "/cameraData.jde";
 	this->cycle = 1000.0/freq;
@@ -27,13 +44,38 @@
 
 poolWriteImages::~poolWriteImages() {
 	this->outfile.close();
-	// TODO Auto-generated destructor stub
+	if (mBuffer)
+		delete(mBuffer);
 }
 
 bool poolWriteImages::getActive(){
 	return this->active;
 }
 
+bool poolWriteImages::startCustomLog (std::string name, int seconds)
+{
+	bool ret;
+
+	pthread_mutex_lock(&(this->mModeMutex));
+
+	if (mMode != SAVE_BUFFER)
+	{
+		jderobot::Logger::getInstance()->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;
+}
+
 void poolWriteImages::consumer_thread(){
 //	while(this->active){
 		//std::cout << "consumidor entro" << std::endl;
@@ -52,10 +94,111 @@
 			pthread_mutex_unlock(&(this->mutex));
 
 			std::stringstream buff;//create a stringstream
+			buff << "data/images/camera" << cameraID << "/" << relative << "." << imageFormat;
 
-			buff << "data/images/camera" << cameraID << "/" << relative << "." << imageFormat;
-			cv::imwrite(buff.str(), img2Save,this->compression_params);
-			this->outfile << relative<< std::endl;
+
+			std::stringstream dataPath;
+			dataPath << "data-" << mNameLog << "/images/camera" << this->cameraID << "/";
+
+			MODE currentMode;
+			pthread_mutex_lock(&(this->mModeMutex));
+			currentMode = mMode;
+			pthread_mutex_unlock(&(this->mModeMutex));
+
+			// Normal mode
+			if (currentMode == WRITE_FRAME)
+			{
+
+				cv::imwrite(buff.str(), img2Save,this->compression_params);
+				this->outfile << relative<< std::endl;
+
+			}
+			else
+			{
+				// Save buffer in memory mode == SAVE_BUFFER
+
+				RingBuffer::RingNode node;
+				node.cameraId = cameraID;
+				node.relativeTime = relative;
+
+				img2Save.copyTo(node.frame);
+				mBuffer->addNode(node);
+
+				if (currentMode == WRITE_BUFFER)
+				{
+
+					//Create dir
+					boost::filesystem::path dir(dataPath.str());
+					boost::filesystem::create_directories(dir);
+
+					std::stringstream filePath;
+					filePath << "data-" << mNameLog << "/images/camera" << this->cameraID << "/cameraData.jde";
+					this->logfile.open(filePath.str().c_str());
+
+
+					jderobot::Logger::getInstance()->info("Init recording log: " + mNameLog + " (camera" + boost::lexical_cast<std::string>(this->cameraID) +  " ) with "
+							+ boost::lexical_cast<std::string>(mBufferSeconds)
+							+ " buffer seconds and " + boost::lexical_cast<std::string>(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<int> res;
+
+						boost::filesystem::directory_iterator end_itr;
+						for ( boost::filesystem::directory_iterator itr( dataPath.str() ); 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)
+							{
+								jderobot::Logger::getInstance()->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<int>::iterator it = res.begin(); it < res.end(); it++)
+						{
+							this->logfile << *it << std::endl;
+						}
+
+						this->logfile.close();
+						jderobot::Logger::getInstance()->info("End recording log: " + mNameLog );
+
+						pthread_mutex_lock(&(this->mModeMutex));
+						mMode = SAVE_BUFFER;
+						pthread_mutex_unlock(&(this->mModeMutex));
+					}
+					else
+					{
+						std::stringstream path;
+						path << "data-" << mNameLog << "/images/camera" << cameraID << "/" << relative << "." << imageFormat;
+						cv::imwrite(path.str(), img2Save,this->compression_params);
+					}
+				}
+			}
+
 		}
 		else
 			pthread_mutex_unlock(&(this->mutex));

Modified: trunk/src/stable/components/recorder/poolWriteImages.h
===================================================================
--- trunk/src/stable/components/recorder/poolWriteImages.h	2014-05-27 17:02:31 UTC (rev 1231)
+++ trunk/src/stable/components/recorder/poolWriteImages.h	2014-05-27 17:15:22 UTC (rev 1232)
@@ -32,22 +32,35 @@
 #include <jderobot/camera.h>
 #include <visionlib/colorspaces/colorspacesmm.h>
 #include <fstream>
+#include <boost/filesystem.hpp>
 
+#include "RingBuffer.h"
 
-
 namespace recorder{
 
 
 class poolWriteImages {
 public:
-	poolWriteImages(jderobot::CameraPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat,  std::vector<int> compression_params);
+
+	enum MODE
+		{
+			WRITE_FRAME = 0,
+			SAVE_BUFFER,
+			WRITE_BUFFER,
+			WRITE_END_LOG
+		};
+
+	poolWriteImages(jderobot::CameraPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat,  std::vector<int> compression_params, MODE mode, int bufferSeconds);
 	virtual ~poolWriteImages();
 	bool getActive();
 	//void produceImage(cv::Mat image, long long int it);
 	void consumer_thread();
 	void producer_thread( struct timeval inicio);
 
+	bool startCustomLog(std::string name, int seconds);
 
+
+
 private:
 	pthread_mutex_t mutex;
 	std::vector<cv::Mat> images;
@@ -63,7 +76,17 @@
 	jderobot::CameraPrx prx;
 	std::ofstream outfile;
 
+	// write log by demand
+	RingBuffer* mBuffer;
+	pthread_mutex_t mModeMutex;
+	std::string mNameLog;
+	int mLastSecondsLog;
+	int mBufferSeconds;
 
+	MODE mMode;
+	boost::posix_time::ptime mFinalInit, mFinalEnd;
+	std::ofstream logfile;
+
 	//threads
 
 };

Modified: trunk/src/stable/components/recorder/recorder.cfg
===================================================================
--- trunk/src/stable/components/recorder/recorder.cfg	2014-05-27 17:02:31 UTC (rev 1231)
+++ trunk/src/stable/components/recorder/recorder.cfg	2014-05-27 17:15:22 UTC (rev 1232)
@@ -1,21 +1,21 @@
 Recorder.FileName=datos.jde
-Recorder.nCameras=4
-Recorder.nDethSensors=2
+Recorder.nCameras=1
+Recorder.nDethSensors=0
 Recorder.nLasers=0
 Recorder.DepthSensor1.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
 Recorder.DepthSensor2.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
 Recorder.Laser1.Proxy=Laser:tcp -h localhost -p 9996
-Recorder.Camera1.Proxy=cameraA:tcp -h localhost -p 9998
-Recorder.Camera2.Proxy=cameraB:tcp -h localhost -p 9998
+Recorder.Camera1.Proxy=cameraA:tcp -h localhost -p 9999
+Recorder.Camera2.Proxy=cameraB:tcp -h localhost -p 9999
 Recorder.Camera3.Proxy=cameraA:tcp -h localhost -p 9998
 Recorder.Camera4.Proxy=cameraB:tcp -h localhost -p 9998
-Recorder.nPose3dEncoders=2
+Recorder.nPose3dEncoders=0
 Recorder.Pose3DEncoders1.Proxy=Pose3DEncoders1:tcp -h localhost -p 9993
 Recorder.Pose3DEncoders2.Proxy=pose3dencoders2:tcp -h localhost -p 9999
 Recorder.nEncoders=0
 Recorder.Encoders1.Proxy=Encoders:tcp -h localhost -p 9997
-Recorder.GUI=1
-Recorder.nConsumers=2
+Recorder.GUI=0
+Recorder.nConsumers=1
 Recorder.poolSize=10
 
 Recorder.Hostname=localhost
@@ -27,3 +27,9 @@
 # from 0 to 100 (the higher is the better)
 Recorder.Laser.Samples=180
 Recorder.Hz=20
+
+
+Recorder.Buffer.Enabled=0
+Recorder.Buffer.Seconds=60
+Recorder.Endpoints=default -h 0.0.0.0 -p 9992
+Recorder.Name=Recorder1

Modified: trunk/src/stable/components/recorder/recorder.cpp
===================================================================
--- trunk/src/stable/components/recorder/recorder.cpp	2014-05-27 17:02:31 UTC (rev 1231)
+++ trunk/src/stable/components/recorder/recorder.cpp	2014-05-27 17:15:22 UTC (rev 1232)
@@ -13,6 +13,7 @@
 #include <jderobot/pose3dmotors.h>
 #include <jderobot/pose3dencoders.h>
 #include <jderobot/pointcloud.h>
+#include <jderobot/recorder.h>
 #include <visionlib/colorspaces/colorspacesmm.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
@@ -178,6 +179,9 @@
 }
 
 
+std::vector<recorder::poolWriteImages*> poolImages;
+Ice::ObjectAdapterPtr adapter;
+
 void exitApplication(int s){
 	globalActive=false;
 	killed=true;
@@ -230,11 +234,38 @@
 			system(instruction2.str().c_str());
 	}
 
+	for (int i=0; i< poolImages.size(); i++)
+	{
+		delete(poolImages[i]);
+	}
+
    if (ic)
 	   ic->destroy();
 }
 
 
+
+class RecorderI: virtual public jderobot::recorder
+{
+public:
+
+	RecorderI() {}
+
+	virtual bool saveLog(const ::std::string& name, ::Ice::Int seconds, const ::Ice::Current& ic )
+	{
+
+		bool ret = true;
+		for (int i=0; i< poolImages.size(); i++)
+		{
+			bool log = poolImages[i]->startCustomLog(name, seconds);
+			ret = ret && log;
+		}
+
+		return ret;
+	}
+};
+
+
 int main(int argc, char** argv){
 
 
@@ -275,7 +306,7 @@
 
 	pthread_attr_t attr;
 	//images
-	std::vector<recorder::poolWriteImages*> poolImages;
+
 	int nConsumidores;
 	int poolSize;
 	//lasers
@@ -295,6 +326,7 @@
 	int jpgQuality;
 	std::string imageFormat;
 	std::vector<int> compression_params;
+
     
    
 	//---------------- INPUT ARGUMENTS ---------------//
@@ -355,6 +387,10 @@
 
 		}
 
+
+		int bufferEnabled = prop->getPropertyAsIntWithDefault("Recorder.Buffer.Enabled",0);
+		int bufferSeconds = prop->getPropertyAsIntWithDefault("Recorder.Buffer.Seconds",0);
+
 		for (int i=0; i< nCameras; i++){
 			struct stat buf;
 			std::stringstream cameraPath;
@@ -381,11 +417,13 @@
 				cprx.push_back(cprxAux);
 
 			//pool
-			recorder::poolWriteImages *temp = new recorder::poolWriteImages(cprxAux, Hz,poolSize,i+1,imageFormat,compression_params);
+			recorder::poolWriteImages *temp = new recorder::poolWriteImages(cprxAux, Hz,poolSize,i+1,imageFormat,
+					compression_params, (bufferEnabled == 0)? recorder::poolWriteImages::WRITE_FRAME : recorder::poolWriteImages::SAVE_BUFFER, bufferSeconds);
 			poolImages.push_back(temp);
 
 
 			for (int j=i*nConsumidores; j< i*nConsumidores+nConsumidores; j++){
+				std::cout << "Create consumer" << std::endl;
 				pthread_create(&consumerThreads[j], &attr, camera_pool_consumer_thread,temp);
 				totalConsumers++;
 			}
@@ -393,8 +431,21 @@
 			totalProducers++;
 		}
 
+		if (bufferEnabled)
+		{
+			// Analyze EndPoint
+			std::string Endpoints = prop->getProperty("Recorder.Endpoints");
+			adapter =ic->createObjectAdapterWithEndpoints("Recorder", Endpoints);
+			std::string name = prop->getProperty("Recorder.Name");
+			jderobot::Logger::getInstance()->info("Creating Recorder: " + name);
+			RecorderI* recorder_prx = new RecorderI();
+			adapter->add(recorder_prx, ic->stringToIdentity(name));
 
+			adapter->activate();
+		}
 
+
+
 		nLasers= prop->getPropertyAsInt("Recorder.nLasers");
 		if (nLasers > 0){
 			struct stat buf;
@@ -615,7 +666,8 @@
 
 				gettimeofday(&b,NULL);
 				totalb=b.tv_sec*1000000+b.tv_usec;
-				std::cout << "Recorder takes " << (totalb-totala)/1000 << " ms" << std::endl;
+				if (!bufferEnabled)
+					std::cout << "Recorder takes " << (totalb-totala)/1000 << " ms" << std::endl;
 
 				diff = (totalb-totala)/1000;
 				if(diff < 0 || diff > cycle)



More information about the Jderobot-admin mailing list