[Jderobot-admin] jderobot-r1093 - trunk/src/stable/libs/parallelIce

frivas en jderobot.org frivas en jderobot.org
Vie Nov 8 17:11:21 CET 2013


Author: frivas
Date: 2013-11-08 17:11:21 +0100 (Fri, 08 Nov 2013)
New Revision: 1093

Added:
   trunk/src/stable/libs/parallelIce/laserClient.cpp
   trunk/src/stable/libs/parallelIce/laserClient.h
Modified:
   trunk/src/stable/libs/parallelIce/cameraClient.cpp
   trunk/src/stable/libs/parallelIce/cameraClient.h
   trunk/src/stable/libs/parallelIce/pointcloudClient.cpp
   trunk/src/stable/libs/parallelIce/pointcloudClient.h
Log:
#110 included pause-resume methods to parallelIce and included laser support


Modified: trunk/src/stable/libs/parallelIce/cameraClient.cpp
===================================================================
--- trunk/src/stable/libs/parallelIce/cameraClient.cpp	2013-11-08 09:40:08 UTC (rev 1092)
+++ trunk/src/stable/libs/parallelIce/cameraClient.cpp	2013-11-08 16:11:21 UTC (rev 1093)
@@ -53,6 +53,7 @@
 		std::cout << prefix + " Not camera provided" << std::endl;
 	}
 	_done=false;
+	this->pauseStatus=false;
 }
 
 cameraClient::~cameraClient() {
@@ -60,6 +61,19 @@
 	_done=true;
 }
 
+
+void cameraClient::pause(){
+	this->pauseStatus=true;
+}
+
+void cameraClient::resume(){
+	this->controlMutex.lock();
+		this->pauseStatus=false;
+		this->sem.broadcast();
+	this->controlMutex.unlock();
+}
+
+
 void
 cameraClient::run(){
 	jderobot::ImageDataPtr dataPtr;
@@ -68,6 +82,11 @@
 
 	last=IceUtil::Time::now();
 	while (!(_done)){
+		if (pauseStatus){
+			IceUtil::Mutex::Lock sync(this->controlMutex);
+			this->sem.wait(sync);
+		}
+
 		dataPtr = this->prx->getImageData();
 		fmt = colorspaces::Image::Format::searchFormat(dataPtr->description->format);
 		if (!fmt)
@@ -83,7 +102,7 @@
 		this->controlMutex.unlock();
 
 		if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) > this->cycle ){
-			//if (this->debug)
+			if (this->debug)
 				std::cout<<"--------" << prefix << " adquisition timeout-" << std::endl;
 		}
 		else{

Modified: trunk/src/stable/libs/parallelIce/cameraClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/cameraClient.h	2013-11-08 09:40:08 UTC (rev 1092)
+++ trunk/src/stable/libs/parallelIce/cameraClient.h	2013-11-08 16:11:21 UTC (rev 1093)
@@ -43,6 +43,10 @@
 	//callbacks
 	cv::Mat getImage();
 	int getRefreshRate(){return refreshRate;};
+	void pause();
+	void resume();
+	bool getPause(){return pauseStatus;};
+
 private:
 	cv::Mat data;
 	jderobot::CameraPrx prx;
@@ -53,7 +57,10 @@
 	bool debug;
 	bool _done;
 	int refreshRate;
+	bool pauseStatus;
 
+	IceUtil::Cond sem;
+
 };
 
 } /* namespace jderobot */

Added: trunk/src/stable/libs/parallelIce/laserClient.cpp
===================================================================
--- trunk/src/stable/libs/parallelIce/laserClient.cpp	                        (rev 0)
+++ trunk/src/stable/libs/parallelIce/laserClient.cpp	2013-11-08 16:11:21 UTC (rev 1093)
@@ -0,0 +1,100 @@
+/*
+ * laserClient.cpp
+ *
+ *  Created on: 23/07/2013
+ *      Author: frivas
+ */
+
+#include "laserClient.h"
+
+namespace jderobot {
+
+laserClient::laserClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug) {
+	// TODO Auto-generated constructor stub
+	this->prefix=prefix;
+	this->debug= debug;
+	Ice::PropertiesPtr prop;
+	prop = ic->getProperties();
+
+	int fps=prop->getPropertyAsIntWithDefault(prefix+"Fps",10);
+	this->cycle=(float)(1/(float)fps)*1000000;
+	try{
+		Ice::ObjectPrx basePointCloud = ic->propertyToProxy(prefix+"Proxy");
+		if (0==basePointCloud){
+			throw prefix + " Could not create proxy";
+		}
+		else {
+			this->prx = jderobot::LaserPrx::checkedCast(basePointCloud);
+			if (0==this->prx)
+				throw "Invalid proxy" + prefix;
+
+		}
+	}catch (const Ice::Exception& ex) {
+		std::cerr << ex << std::endl;
+	}
+	catch (const char* msg) {
+		std::cerr << msg << std::endl;
+		std::cout <<  prefix + " Not laser provided" << std::endl;
+	}
+	_done=false;
+
+}
+
+laserClient::~laserClient() {
+	// TODO Auto-generated destructor stub
+	this->_done=true;
+}
+
+
+void laserClient::pause(){
+	this->pauseStatus=true;
+}
+
+void laserClient::resume(){
+	this->controlMutex.lock();
+		this->pauseStatus=false;
+		this->sem.broadcast();
+	this->controlMutex.unlock();
+}
+
+void laserClient::run(){
+
+	IceUtil::Time last;
+
+	last=IceUtil::Time::now();
+	while (!(_done)){
+		if (pauseStatus){
+			IceUtil::Mutex::Lock sync(this->controlMutex);
+			this->sem.wait(sync);
+		}
+
+		jderobot::LaserDataPtr localLaser=this->prx->getLaserData();
+
+		this->controlMutex.lock();
+		this->data.resize(localLaser->distanceData.size());
+		std::copy( localLaser->distanceData.begin(), localLaser->distanceData.end(), this->data.begin() );
+
+		this->controlMutex.unlock();
+
+		if ((IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()) > this->cycle ){
+			if (this->debug)
+				std::cout<< prefix << ": pointCloud adquisition timeout-" << std::endl;
+		}
+		else{
+			usleep(this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
+		}
+		this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
+		last=IceUtil::Time::now();
+	}
+}
+
+std::vector<int>  laserClient::getData(){
+	std::vector<int> laser;
+	this->controlMutex.lock();
+	laser.resize(this->data.size());
+	std::copy( this->data.begin(), this->data.end(), laser.begin() );
+	this->controlMutex.unlock();
+	return laser;
+}
+
+} /* namespace eldercare */

Added: trunk/src/stable/libs/parallelIce/laserClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/laserClient.h	                        (rev 0)
+++ trunk/src/stable/libs/parallelIce/laserClient.h	2013-11-08 16:11:21 UTC (rev 1093)
@@ -0,0 +1,67 @@
+/*
+ *  Copyright (C) 1997-2013 JDE Developers TeamkinectViewer.camRGB
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/.
+ *
+ *  Author : Jose María Cañas <jmplaza en gsyc.es>
+			Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>
+
+ */
+
+#ifndef LASERCLIENT_H_
+#define LASERCLIENT_H_
+
+#include <IceUtil/IceUtil.h>
+#include <iostream>
+#include <Ice/Ice.h>
+#include <jderobot/laser.h>
+#include <cv.h>
+#include <sstream>
+#include <fstream>
+
+namespace jderobot {
+
+
+class laserClient: public IceUtil::Thread {
+public:
+	laserClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug);
+	virtual ~laserClient();
+	virtual void run();
+
+	std::vector<int>  getData();
+	int getRefreshRate(){return refreshRate;};
+	void pause();
+	void resume();
+	bool getPause(){return pauseStatus;};
+
+
+
+private:
+	std::string prefix;
+	std::vector<int> data;
+	jderobot::LaserPrx prx;
+	long long int cycle;
+	IceUtil::Mutex controlMutex;
+	bool debug;
+	bool _done;
+	int refreshRate;
+	bool pauseStatus;
+
+	IceUtil::Cond sem;
+
+};
+
+
+} /* namespace jderobot */
+#endif /* LASERCLIENT_H_ */

Modified: trunk/src/stable/libs/parallelIce/pointcloudClient.cpp
===================================================================
--- trunk/src/stable/libs/parallelIce/pointcloudClient.cpp	2013-11-08 09:40:08 UTC (rev 1092)
+++ trunk/src/stable/libs/parallelIce/pointcloudClient.cpp	2013-11-08 16:11:21 UTC (rev 1093)
@@ -50,6 +50,7 @@
 		std::cout <<  prefix + " Not camera provided" << std::endl;
 	}
 	_done=false;
+	this->pauseStatus=false;
 
 }
 
@@ -58,6 +59,18 @@
 	this->_done=true;
 }
 
+void pointcloudClient::pause(){
+	this->pauseStatus=true;
+}
+
+void pointcloudClient::resume(){
+	this->controlMutex.lock();
+		this->pauseStatus=false;
+		this->sem.broadcast();
+	this->controlMutex.unlock();
+}
+
+
 void pointcloudClient::run(){
 
 	IceUtil::Time last;
@@ -65,8 +78,11 @@
 	last=IceUtil::Time::now();
 
 	while (!(_done)){
+		if (pauseStatus){
+			IceUtil::Mutex::Lock sync(this->controlMutex);
+			this->sem.wait(sync);
+		}
 
-
 		jderobot::pointCloudDataPtr localCloud=this->prx->getCloudData();
 
 		this->controlMutex.lock();

Modified: trunk/src/stable/libs/parallelIce/pointcloudClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/pointcloudClient.h	2013-11-08 09:40:08 UTC (rev 1092)
+++ trunk/src/stable/libs/parallelIce/pointcloudClient.h	2013-11-08 16:11:21 UTC (rev 1093)
@@ -41,6 +41,9 @@
 
 	std::vector<jderobot::RGBPoint>  getData();
 	int getRefreshRate(){return refreshRate;};
+	void pause();
+	void resume();
+	bool getPause(){return pauseStatus;};
 
 
 private:
@@ -52,7 +55,10 @@
 	bool debug;
 	bool _done;
 	int refreshRate;
+	bool pauseStatus;
 
+	IceUtil::Cond sem;
+
 };
 
 } /* namespace jderobot */



More information about the Jderobot-admin mailing list