[Jderobot-admin] jderobot-r1217 - in trunk/src/stable: components/pclRGBDServer components/rgbdManualCalibrator components/rgbdViewer libs/parallelIce

frivas en jderobot.org frivas en jderobot.org
Mie Mar 26 19:00:12 CET 2014


Author: frivas
Date: 2014-03-26 19:00:12 +0100 (Wed, 26 Mar 2014)
New Revision: 1217

Modified:
   trunk/src/stable/components/pclRGBDServer/kinectServer.cpp
   trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp
   trunk/src/stable/components/rgbdManualCalibrator/view.cpp
   trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp
   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:
[#206] updated the parallelIce api


Modified: trunk/src/stable/components/pclRGBDServer/kinectServer.cpp
===================================================================
--- trunk/src/stable/components/pclRGBDServer/kinectServer.cpp	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/components/pclRGBDServer/kinectServer.cpp	2014-03-26 18:00:12 UTC (rev 1217)
@@ -15,7 +15,7 @@
 #include <pcl/io/openni_grabber.h>
 #include <pcl/io/openni_camera/openni_driver.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/visualization/cloud_viewer.h>
+//#include <pcl/visualization/cloud_viewer.h>
 #include <pcl/filters/voxel_grid.h>
 
 #include "cameradepth.h"

Modified: trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp	2014-03-26 18:00:12 UTC (rev 1217)
@@ -59,9 +59,9 @@
 			std::ostringstream sTemp;
 			std::ostringstream sTemp2;
 			sTemp << "rgbdManualCalibrator.CameraRGB." << i << ".";
-			sources[i].RGB=new jderobot::cameraClient(ic, sTemp.str(),false);
+			sources[i].RGB=new jderobot::cameraClient(ic, sTemp.str());
 			sTemp2 << "rgbdManualCalibrator.CameraDEPTH." << i << ".";
-			sources[i].DEPTH=new jderobot::cameraClient(ic, sTemp2.str(),false);
+			sources[i].DEPTH=new jderobot::cameraClient(ic, sTemp2.str());
 
 
 			if (sources[i].RGB==NULL)

Modified: trunk/src/stable/components/rgbdManualCalibrator/view.cpp
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/view.cpp	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/components/rgbdManualCalibrator/view.cpp	2014-03-26 18:00:12 UTC (rev 1217)
@@ -151,7 +151,8 @@
 		if (w_depth->get_active()){
 
 
-			cv::Mat srcDEPTH=sources[cam].DEPTH->getImage();
+			cv::Mat srcDEPTH;
+			sources[cam].DEPTH->getImage(srcDEPTH);
 
 
 			/*split channels to separate distance from image*/
@@ -179,7 +180,8 @@
 		}
 		else{
 
-			cv::Mat rgb = sources[cam].RGB->getImage();
+			cv::Mat rgb;
+			sources[cam].RGB->getImage(rgb);
 
 			controller->drawWorld(rgb,cam);
 			Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*)rgb.data,
@@ -210,7 +212,8 @@
 			this->world->clear_points();
 			for (int pos=0; pos< nCameras; pos++){
 				int colour;
-				cv::Mat srcDEPTH =sources[pos].DEPTH->getImage();
+				cv::Mat srcDEPTH;
+				sources[pos].DEPTH->getImage(srcDEPTH);
 
 				cv::Mat distance(srcDEPTH.rows, srcDEPTH.cols, CV_32FC1);
 				//split channels to separate distance from image
@@ -221,7 +224,8 @@
 					for (int y=0; y<layers[1].rows; y++){
 						distance.at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x);					}
 				}
-				cv::Mat rgb = sources[pos].RGB->getImage();
+				cv::Mat rgb;
+				sources[pos].RGB->getImage(rgb);
 				if (this->trueColor)
 					colour=0;
 				else
@@ -236,7 +240,8 @@
 		}
 		else{
 			this->world->clear_points();
-			cv::Mat srcDEPTH =sources[cam].DEPTH->getImage();
+			cv::Mat srcDEPTH;
+			sources[cam].DEPTH->getImage(srcDEPTH);
 
 			cv::Mat distance(srcDEPTH.rows, srcDEPTH.cols, CV_32FC1);
 			//split channels to separate distance from image
@@ -248,7 +253,8 @@
 					distance.at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x);					}
 			}
 
-			cv::Mat rgb = sources[cam].RGB->getImage();
+			cv::Mat rgb;
+			sources[cam].RGB->getImage(rgb);
 			this->controller->add_depth_pointsImage(distance,rgb, world, cam,5,0);
 
 

Modified: trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp	2014-03-26 18:00:12 UTC (rev 1217)
@@ -56,11 +56,11 @@
 		lastIT=IceUtil::Time::now();
 		while(rgbdViewergui_ptx->isVisible() && ! rgbdViewergui_ptx->isClosed()){
 			if (camRGB!=NULL)
-				camRGB->getImage().copyTo(rgb);
+				camRGB->getImage(rgb);
 			if (camDEPTH!=NULL)
-				camDEPTH->getImage().copyTo(depth);
+				camDEPTH->getImage(depth);
 			if (pcClient!=NULL)
-				cloud=pcClient->getData();
+				pcClient->getData(cloud);
 
 
 			if ((rgb.rows!=0)&&(depth.rows!=0)){
@@ -133,7 +133,7 @@
 		return 1;
 	}
 	if (prop->getPropertyAsIntWithDefault("rgbdViewer.CameraRGBActive",0)){
-		camRGB = new jderobot::cameraClient(ic,"rgbdViewer.CameraRGB.",false);
+		camRGB = new jderobot::cameraClient(ic,"rgbdViewer.CameraRGB.");
 		if (camRGB != NULL){
 			rgbCamSelected=true;
 			camRGB->start();
@@ -145,7 +145,7 @@
 
 	}
 	if (prop->getPropertyAsIntWithDefault("rgbdViewer.CameraDEPTHActive",0)){
-		camDEPTH = new jderobot::cameraClient(ic,"rgbdViewer.CameraDEPTH.",false);
+		camDEPTH = new jderobot::cameraClient(ic,"rgbdViewer.CameraDEPTH.");
 		if (camDEPTH != NULL){
 			depthCamSelected=true;
 			camDEPTH->start();
@@ -158,7 +158,7 @@
 
 
 	if (prop->getPropertyAsIntWithDefault("rgbdViewer.pointCloudActive",0)){
-		pcClient = new jderobot::pointcloudClient(ic,"rgbdViewer.pointCloud.",false);
+		pcClient = new jderobot::pointcloudClient(ic,"rgbdViewer.pointCloud.");
 		if (pcClient!= NULL){
 			pcClient->start();
 			pointCloudSelected=true;

Modified: trunk/src/stable/libs/parallelIce/cameraClient.cpp
===================================================================
--- trunk/src/stable/libs/parallelIce/cameraClient.cpp	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/libs/parallelIce/cameraClient.cpp	2014-03-26 18:00:12 UTC (rev 1217)
@@ -24,10 +24,9 @@
 
 namespace jderobot {
 
-cameraClient::cameraClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug) {
+cameraClient::cameraClient(Ice::CommunicatorPtr ic, std::string prefix) {
 
 	this->prefix=prefix;
-	this->debug=debug;
 	Ice::PropertiesPtr prop;
 	prop = ic->getProperties();
 	Ice::ObjectPrx baseCamera;
@@ -61,10 +60,9 @@
 	this->pauseStatus=false;
 }
 
-cameraClient::cameraClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug, std::string proxy){
+cameraClient::cameraClient(Ice::CommunicatorPtr ic, std::string prefix, std::string proxy){
 
 	this->prefix=prefix;
-	this->debug=debug;
 	Ice::PropertiesPtr prop;
 	prop = ic->getProperties();
 	Ice::ObjectPrx baseCamera;
@@ -101,7 +99,6 @@
 }
 
 cameraClient::~cameraClient() {
-	// TODO Auto-generated destructor stub
 	_done=true;
 }
 
@@ -145,14 +142,10 @@
 			throw "Format not supported";
 		colorspaces::Image imageRGB(dataPtr->description->width,dataPtr->description->height,fmt,&(dataPtr->pixelData[0]));
 		colorspaces::ImageRGB8 img_rgb888(imageRGB);//conversion will happen if needed
-		cv::Mat localDataPtr = cv::Mat(cvSize(img_rgb888.width,img_rgb888.height), CV_8UC3, img_rgb888.data);
-		cv::Mat localData;
-		localDataPtr.copyTo(localData);
 		this->controlMutex.lock();
-		this->data.release();
-		localData.copyTo(this->data);
+		cv::Mat(cvSize(img_rgb888.width,img_rgb888.height), CV_8UC3, img_rgb888.data).copyTo(this->data);
 		this->controlMutex.unlock();
-
+		img_rgb888.release();
 		int process = this->cycle - (IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds());
 
 		if (process > (int)cycle ){
@@ -179,6 +172,7 @@
 		}
 
 	}
+	this->data.release();
 }
 
 void cameraClient::stop_thread()
@@ -186,13 +180,10 @@
 	_done = true;
 }
 
-cv::Mat cameraClient::getImage(){
-	cv::Mat local;
+void cameraClient::getImage(cv::Mat& image){
 	this->controlMutex.lock();
-	this->data.copyTo(local);
+	this->data.copyTo(image);
 	this->controlMutex.unlock();
-	return local;
-
 }
 
 } /* namespace jderobot */

Modified: trunk/src/stable/libs/parallelIce/cameraClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/cameraClient.h	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/libs/parallelIce/cameraClient.h	2014-03-26 18:00:12 UTC (rev 1217)
@@ -37,13 +37,13 @@
 
 class cameraClient: public IceUtil::Thread {
 public:
-	cameraClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug);
-	cameraClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug, std::string proxy);
+	cameraClient(Ice::CommunicatorPtr ic, std::string prefix);
+	cameraClient(Ice::CommunicatorPtr ic, std::string prefix, std::string proxy);
 	virtual ~cameraClient();
 	virtual void run();
 
 	//callbacks
-	cv::Mat getImage();
+	void getImage(cv::Mat& image);
 	int getRefreshRate(){return refreshRate;};
 	void pause();
 	void resume();
@@ -59,7 +59,6 @@
 	//int type; //0 color 1 depth
 	IceUtil::Mutex controlMutex;
 	std::string prefix;
-	bool debug;
 	bool _done;
 	int refreshRate;
 	bool pauseStatus;

Modified: trunk/src/stable/libs/parallelIce/pointcloudClient.cpp
===================================================================
--- trunk/src/stable/libs/parallelIce/pointcloudClient.cpp	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/libs/parallelIce/pointcloudClient.cpp	2014-03-26 18:00:12 UTC (rev 1217)
@@ -22,10 +22,9 @@
 
 namespace jderobot {
 
-pointcloudClient::pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug) {
+pointcloudClient::pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix) {
 	// TODO Auto-generated constructor stub
 	this->prefix=prefix;
-	this->debug= debug;
 	Ice::PropertiesPtr prop;
 	prop = ic->getProperties();
 	this->refreshRate=0;
@@ -55,10 +54,9 @@
 
 }
 
-pointcloudClient::pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug, std::string proxy)
+pointcloudClient::pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix, std::string proxy)
 {
 	this->prefix=prefix;
-	this->debug= debug;
 	Ice::PropertiesPtr prop;
 	prop = ic->getProperties();
 	this->refreshRate=0;
@@ -144,16 +142,18 @@
 
 		this->refreshRate=(int)(1000000/(IceUtil::Time::now().toMicroSeconds() - last.toMicroSeconds()));
 		last=IceUtil::Time::now();
+		usleep(100);
 	}
+	this->data.clear();
+
 }
 
-std::vector<jderobot::RGBPoint>  pointcloudClient::getData(){
-	std::vector<jderobot::RGBPoint> cloud;
+void  pointcloudClient::getData(std::vector<jderobot::RGBPoint>& cloud){
 	this->controlMutex.lock();
 	cloud.resize(this->data.size());
 	std::copy( this->data.begin(), this->data.end(), cloud.begin() );
 	this->controlMutex.unlock();
-	return cloud;
+	this->data.clear();
 }
 
 } /* namespace jderobot */

Modified: trunk/src/stable/libs/parallelIce/pointcloudClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/pointcloudClient.h	2014-03-25 15:30:02 UTC (rev 1216)
+++ trunk/src/stable/libs/parallelIce/pointcloudClient.h	2014-03-26 18:00:12 UTC (rev 1217)
@@ -36,13 +36,13 @@
 
 class pointcloudClient: public IceUtil::Thread {
 public:
-	pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug);
-	pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix, bool debug, std::string proxy);
+	pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix);
+	pointcloudClient(Ice::CommunicatorPtr ic, std::string prefix, std::string proxy);
 
 	virtual ~pointcloudClient();
 	virtual void run();
 
-	std::vector<jderobot::RGBPoint>  getData();
+	void getData(std::vector<jderobot::RGBPoint>& cloud);
 	int getRefreshRate(){return refreshRate;};
 	void pause();
 	void resume();
@@ -56,7 +56,6 @@
 	jderobot::pointCloudPrx prx;
 	long long int cycle;
 	IceUtil::Mutex controlMutex;
-	bool debug;
 	bool _done;
 	int refreshRate;
 	bool pauseStatus;



More information about the Jderobot-admin mailing list