[Jderobot-admin] jderobot-r1048 - in trunk/src/stable/components: . pclRGBDServer

ahcorde en jderobot.org ahcorde en jderobot.org
Lun Oct 14 17:41:26 CEST 2013


Author: ahcorde
Date: 2013-10-14 17:41:26 +0200 (Mon, 14 Oct 2013)
New Revision: 1048

Added:
   trunk/src/stable/components/pclRGBDServer/
   trunk/src/stable/components/pclRGBDServer/CMakeLists.txt
   trunk/src/stable/components/pclRGBDServer/cameraRGB.h
   trunk/src/stable/components/pclRGBDServer/cameradepth.h
   trunk/src/stable/components/pclRGBDServer/kinectServer.cfg
   trunk/src/stable/components/pclRGBDServer/kinectServer.cpp
   trunk/src/stable/components/pclRGBDServer/pointcloud.h
Log:
#53 pclRGBDServer
 - Remove dependencies with gearbox
 - Integrate in JDErobot compilation


Added: trunk/src/stable/components/pclRGBDServer/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/pclRGBDServer/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/pclRGBDServer/CMakeLists.txt	2013-10-14 15:41:26 UTC (rev 1048)
@@ -0,0 +1,27 @@
+
+if(with_pcl)
+
+    SET( SOURCE_FILES kinectServer.cpp)
+
+    include_directories(
+        ${INTERFACES_CPP_DIR}
+        ${LIBS_DIR}/
+        ${CMAKE_CURRENT_SOURCE_DIR}
+        /usr/include/vtk-5.8
+    )
+
+    link_directories(${pcl_LIBRARIES})
+    include_directories(${pcl_INCLUDE_DIR})
+
+    add_executable (kinectServer  ${SOURCE_FILES})
+
+    TARGET_LINK_LIBRARIES(kinectServer 
+      ${ZeroCIce_LIBRARIES}
+      ${pcl_LIBRARIES}
+      ${OpenCV_LIBRARIES}
+      JderobotInterfaces
+      jderobotutil
+      ${openni_LIBRARIES}
+    )
+
+endif()

Added: trunk/src/stable/components/pclRGBDServer/cameraRGB.h
===================================================================
--- trunk/src/stable/components/pclRGBDServer/cameraRGB.h	                        (rev 0)
+++ trunk/src/stable/components/pclRGBDServer/cameraRGB.h	2013-10-14 15:41:26 UTC (rev 1048)
@@ -0,0 +1,150 @@
+
+
+namespace kinect{
+
+    pthread_mutex_t mutexRGB;
+    cv::Mat imageRGB;
+    class CamaraRGB: virtual public jderobot::Camera {
+	public:
+		CamaraRGB(std::string& propertyPrefix): prefix(propertyPrefix) {
+		   
+		    imageDescription = (new jderobot::ImageDescription());
+		    //cameraDescription = (new jderobot::CameraDescription());
+		    std::cout << "CameraRGB constructor" << std::endl;
+            replyTask = new ReplyTask(this);
+		    replyTask->start(); // my own thread
+		}
+
+		std::string getName () {
+			return (cameraDescription->name);
+		}
+
+		std::string getRobotName () {
+			return ("");
+		}
+
+		virtual ~CamaraRGB() {
+
+		}
+
+		virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
+			return imageDescription;
+		}
+
+		virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
+			return cameraDescription;
+		}
+
+		virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) {
+			return 0;
+		}
+
+		virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
+			replyTask->pushJob(cb);
+		}
+
+		virtual std::string startCameraStreaming(const Ice::Current&){
+
+		}
+
+		virtual void stopCameraStreaming(const Ice::Current&) {
+
+		}
+
+	private:
+		class ReplyTask:  public IceUtil::Thread {
+			public:
+				ReplyTask(CamaraRGB* camera): mycamera(camera)
+				{
+				}
+
+				void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb)
+				{
+					IceUtil::Mutex::Lock sync(requestsMutex);
+					requests.push_back(cb);
+				}
+             
+
+				virtual void run(){
+                    cv::Mat image;
+
+                    while(!imageRGB.data)
+                        usleep(1000);
+ 				      
+ 				    if(imageRGB.data){
+                        pthread_mutex_lock(&mutexRGB);
+                        image = imageRGB.clone();
+                        pthread_mutex_unlock(&mutexRGB);
+                        mycamera->imageDescription->width = image.cols;
+                        mycamera->imageDescription->height = image.rows;
+                        mycamera->imageDescription->size = image.cols*image.rows*3;
+                        mycamera->imageDescription->format = "RGB8";
+ 				    }
+               
+				
+					jderobot::ImageDataPtr reply(new jderobot::ImageData);
+					reply->description = mycamera->imageDescription;
+					struct timeval a, b;
+					int cycle = 50;
+					long totalb,totala;
+					long diff;
+
+					while(1){		
+						gettimeofday(&a,NULL);
+						totala=a.tv_sec*1000000+a.tv_usec;
+
+						IceUtil::Time t = IceUtil::Time::now();
+						reply->timeStamp.seconds = (long)t.toSeconds();
+						reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+                  
+					    reply->pixelData.resize(image.rows*image.cols*3);
+
+                        pthread_mutex_lock(&mutexRGB);
+                        cv::Mat image = imageRGB.clone();
+                        pthread_mutex_unlock(&mutexRGB);
+
+					    memcpy( &(reply->pixelData[0]), (unsigned char *) image.data, image.rows*image.cols*3);
+
+					    { //critical region start
+						   IceUtil::Mutex::Lock sync(requestsMutex);
+						   while(!requests.empty()) {
+							   jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
+							   requests.pop_front();
+							   cb->ice_response(reply);
+						   }
+					    } //critical region end
+                  
+						gettimeofday(&b,NULL);
+						totalb=b.tv_sec*1000000+b.tv_usec;
+
+						diff = (totalb-totala)/1000;
+						diff = cycle-diff;
+
+						//std::cout << "RGB takes " << diff << " ms " << std::endl;
+
+						if(diff < 33)
+							diff = 33;
+
+
+						//Sleep Algorithm//
+						usleep(diff*1000);
+					}
+					
+				}
+				
+				
+				CamaraRGB* mycamera;
+				IceUtil::Mutex requestsMutex;
+				std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+		};
+
+		typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
+		std::string prefix;
+
+		jderobot::ImageDescriptionPtr imageDescription;
+		jderobot::CameraDescriptionPtr cameraDescription;
+		ReplyTaskPtr replyTask;
+
+	}; // end class CamaraRGB
+
+}

Added: trunk/src/stable/components/pclRGBDServer/cameradepth.h
===================================================================
--- trunk/src/stable/components/pclRGBDServer/cameradepth.h	                        (rev 0)
+++ trunk/src/stable/components/pclRGBDServer/cameradepth.h	2013-10-14 15:41:26 UTC (rev 1048)
@@ -0,0 +1,151 @@
+
+#include <jderobot/camera.h>
+
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace kinect{
+    pthread_mutex_t mutexDepth;
+    cv::Mat imageDepth;
+    class CamaraDepth: virtual public jderobot::Camera {
+	    public:
+		    CamaraDepth(std::string& propertyPrefix) : prefix(propertyPrefix) {
+		       
+                imageDescription = (new jderobot::ImageDescription());
+                //cameraDescription = (new jderobot::CameraDescription());
+
+                replyTask = new ReplyTask(this);
+                replyTask->start(); // my own thread
+		    }
+
+		    std::string getName () {
+			    return (cameraDescription->name);
+		    }
+
+		    std::string getRobotName () {
+			    return ("");
+		    }
+
+		    virtual ~CamaraDepth() {
+
+		    }
+
+		    virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
+			    return imageDescription;
+		    }
+
+		    virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
+			    return cameraDescription;
+		    }
+
+		    virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) {
+			    return 0;
+		    }
+
+		    virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
+			    replyTask->pushJob(cb);
+		    }
+
+		    virtual std::string startCameraStreaming(const Ice::Current&){
+
+		    }
+
+		    virtual void stopCameraStreaming(const Ice::Current&) {
+
+		    }
+
+	    private:
+		    class ReplyTask:  public IceUtil::Thread {
+			    public:
+				    ReplyTask(CamaraDepth* camera):  mycamera(camera) {
+				    }
+
+				    void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
+					    IceUtil::Mutex::Lock sync(requestsMutex);
+					    requests.push_back(cb);
+				    }
+                 
+
+				    virtual void run(){
+                        cv::Mat image;
+
+                        while(!imageDepth.data)
+                            usleep(1000);
+
+                        if(imageDepth.data){
+                            pthread_mutex_lock(&mutexDepth);
+                            image = imageDepth.clone();
+                            pthread_mutex_unlock(&mutexDepth);
+                            mycamera->imageDescription->width = image.cols;
+                            mycamera->imageDescription->height = image.rows;
+                            mycamera->imageDescription->size = image.cols*image.rows*3;
+                            mycamera->imageDescription->format = "RGB8";
+                        }
+                   
+				
+					    jderobot::ImageDataPtr reply(new jderobot::ImageData);
+					    reply->description = mycamera->imageDescription;
+					    struct timeval a, b;
+					    int cycle = 50;
+					    long totalb,totala;
+					    long diff;
+
+                        while(1){					
+                            gettimeofday(&a,NULL);
+                            totala=a.tv_sec*1000000+a.tv_usec;
+
+                            IceUtil::Time t = IceUtil::Time::now();
+                            reply->timeStamp.seconds = (long)t.toSeconds();
+                            reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+
+                            reply->pixelData.resize(image.rows*image.cols*3);
+
+                            pthread_mutex_lock(&mutexDepth);
+                            cv::Mat image = imageDepth.clone();
+                            pthread_mutex_unlock(&mutexDepth);
+
+                            memcpy( &(reply->pixelData[0]), (unsigned char *) image.data, image.rows*image.cols*3);
+
+                            { //critical region start
+                                IceUtil::Mutex::Lock sync(requestsMutex);
+                                while(!requests.empty()) {
+                                    jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
+                                    requests.pop_front();
+                                    cb->ice_response(reply);
+                                }
+                            } //critical region end
+                      
+						    gettimeofday(&b,NULL);
+						    totalb=b.tv_sec*1000000+b.tv_usec;
+
+						    diff = (totalb-totala)/1000;
+						    diff = cycle-diff;
+
+						    //std::cout << "Gazeboserver takes " << diff << " ms " << mycamera->fileName << std::endl;
+
+						    if(diff < 33)
+							    diff = 33;
+						    //Sleep Algorithm//
+						    usleep(diff*1000);
+					    }
+					
+				    }
+				
+				
+				    CamaraDepth* mycamera;
+				    IceUtil::Mutex requestsMutex;
+				    std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+		    };
+
+		    typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
+		    std::string prefix;
+		    jderobot::ImageDescriptionPtr imageDescription;
+		    jderobot::CameraDescriptionPtr cameraDescription;
+		    ReplyTaskPtr replyTask;
+
+	    }; // end class CamaraDepth
+}

Added: trunk/src/stable/components/pclRGBDServer/kinectServer.cfg
===================================================================
--- trunk/src/stable/components/pclRGBDServer/kinectServer.cfg	                        (rev 0)
+++ trunk/src/stable/components/pclRGBDServer/kinectServer.cfg	2013-10-14 15:41:26 UTC (rev 1048)
@@ -0,0 +1,6 @@
+#without registry
+KinectServer.Endpoints=default -h localhost -p 9999
+KinectServer.CameraRGB=1
+KinectServer.CameraDepth=1
+KinectServer.CloudPoints=1
+KinectServer.LeafSize=30

Added: trunk/src/stable/components/pclRGBDServer/kinectServer.cpp
===================================================================
--- trunk/src/stable/components/pclRGBDServer/kinectServer.cpp	                        (rev 0)
+++ trunk/src/stable/components/pclRGBDServer/kinectServer.cpp	2013-10-14 15:41:26 UTC (rev 1048)
@@ -0,0 +1,282 @@
+#define  EIGEN_DONT_VECTORIZE 
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+#include <jderobot/pointcloud.h>
+#include <jderobot/camera.h>
+
+#include <boost/thread/thread.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/common/time.h> //fps calculations 
+#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/filters/voxel_grid.h>
+
+#include "cameradepth.h"
+#include "cameraRGB.h"
+#include "pointcloud.h"
+
+#include <iostream>
+#include <pthread.h>
+
+// opencv Libraries
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+//#include <cv.h>
+//#include <highgui.h>
+
+#define NUM_THREADS     5
+#define MAX_DEPTH 10000
+
+
+
+int avRGB = 0;
+int avDepth = 0;
+int avCloud = 0;
+
+int leafSize = 0.1f;
+
+namespace kinect{
+
+unsigned short depth[10000];
+
+void raw2depth(){
+   int i;
+   for ( i=0; i<MAX_DEPTH; i++) {
+        float v = (float)i/MAX_DEPTH;//for visualization purposes only
+        v = v*v;
+        v = v*36*256;
+        depth[i] = v;
+        //std::cout << "depth[]:" << depth[i] << std::endl;
+   } 
+}
+
+void depth2rgb( const XnDepthPixel*  Xn_disparity, cv::Mat image){
+   int i;
+   //const unsigned short *disparity = Xn_disparity;
+    
+   for (i=0; i<image.rows* image.cols; i++) {
+      //std::cout << i << std::endl;
+      int pval = depth[Xn_disparity[i]];
+      int lb = pval & 0xff;
+      switch (pval>>8) {
+	      case 0:
+		      image.data[3*i+0] = 255;
+		      image.data[3*i+1] = 255-lb;
+		      image.data[3*i+2] = 255-lb;
+		      break;
+	      case 1:
+		      image.data[3*i+0] = 255;
+		      image.data[3*i+1] = lb;
+		      image.data[3*i+2] = 0;
+		      break;
+	      case 2:
+		      image.data[3*i+0] = 255-lb;
+		      image.data[3*i+1] = 255;
+		      image.data[3*i+2] = 0;
+		      break;
+	      case 3:
+		      image.data[3*i+0] = 0;
+		      image.data[3*i+1] = 255;
+		      image.data[3*i+2] = lb;
+		      break;
+	      case 4:
+		      image.data[3*i+0] = 0;
+		      image.data[3*i+1] = 255-lb;
+		      image.data[3*i+2] = 255;
+		      break;
+	      case 5:
+		      image.data[3*i+0] = 0;
+		      image.data[3*i+1] = 0;
+		      image.data[3*i+2] = 255-lb;
+		      break;
+	      default:
+		      image.data[3*i+0] = 0;
+		      image.data[3*i+1] = 0;
+		      image.data[3*i+2] = 0;
+		      break;
+      }
+   }
+}
+
+void rgb_image_cb (const boost::shared_ptr<openni_wrapper::Image>& image)
+{
+   //cout << "Callback RGB--" <<endl;
+   boost::shared_ptr<openni_wrapper::Image> image_w;
+   image_w = image;
+
+   pthread_mutex_lock(&mutexRGB);
+   imageRGB.create(image_w->getHeight(), image_w->getWidth(), CV_8UC3);
+
+   image_w->fillRGB( image_w->getWidth(), image_w->getHeight(),  (unsigned char *) imageRGB.data, imageRGB.step);
+
+   //cv::cvtColor(imageRGB, imageRGB, CV_RGB2BGR);
+   pthread_mutex_unlock(&mutexRGB);
+
+
+}
+
+void depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
+{
+
+
+   boost::shared_ptr<openni_wrapper::DepthImage> image_w;
+   image_w = depth_image;
+   pthread_mutex_lock(&mutexDepth);
+   imageDepth.create(image_w->getHeight(), image_w->getWidth(), CV_8UC3);
+
+   depth2rgb(image_w->getDepthMetaData().Data(), imageDepth);
+   pthread_mutex_unlock(&mutexDepth);
+   
+}
+
+pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudaux(new pcl::PointCloud<pcl::PointXYZRGBA>);
+void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
+{ 
+
+   pthread_mutex_lock(&mutex);
+   
+   float size = 1./leafSize;
+
+   if(cloud->points.size()>0){
+      //viewer.showCloud (cloud); 
+      pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
+      sor.setInputCloud (cloud);
+      sor.setLeafSize (size, size, size);
+      sor.filter (*cloudaux);
+      //std::cout << "Reducidad: "<< cloudaux->points.size() << std::endl;
+      
+      cloud2 = *cloudaux;
+   }
+   pthread_mutex_unlock(&mutex);
+   
+}
+
+
+void* kinectThread(void*)
+{
+    pcl::Grabber* interface = new pcl::OpenNIGrabber();
+   
+    if(avRGB){
+   
+        boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&)> f =
+                              boost::bind (rgb_image_cb, _1);
+        interface->registerCallback (f); 
+    }
+   
+    if(avDepth){                        
+        boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> f_depth =
+                              boost::bind (depth_image_cb, _1);
+        interface->registerCallback (f_depth);  
+    }    
+
+    if(avCloud){                    
+        boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f_cloud =
+                                boost::bind (cloud_cb_, _1);
+
+        interface->registerCallback (f_cloud);                                  
+    } 
+
+    pthread_mutex_init(&mutexRGB, NULL);
+    pthread_mutex_init(&mutex, NULL);
+    pthread_mutex_init(&mutexDepth, NULL);
+
+    interface->start(); 
+    
+    while(1){
+        usleep(100);
+    }  
+}
+
+}
+
+int main(int argc, char** argv) 
+{
+    kinect::raw2depth();
+
+    Ice::ObjectPtr kinect_RGB;
+    Ice::ObjectPtr kinect_Depth;
+    Ice::ObjectPtr kinect_Cloud;
+    
+    Ice::CommunicatorPtr ic;
+    Ice::PropertiesPtr prop;
+    try {
+        //-----------------ICE----------------//
+        ic = Ice::initialize(argc, argv);
+
+        Ice::PropertiesPtr prop = ic->getProperties();
+			
+        avRGB = prop->getPropertyAsInt("KinectServer.CameraRGB");
+
+        std::string Endpoints = prop->getProperty("KinectServer.Endpoints");
+
+        Ice::ObjectAdapterPtr adapter = ic->createObjectAdapterWithEndpoints("kinect", Endpoints);
+
+        if(avRGB){
+            std::string kinectRGB = "kinectRGB";
+            std::cout << "Creating Camera " + kinectRGB << std::endl;
+            kinect_RGB = new kinect::CamaraRGB(kinectRGB);
+            adapter->add(kinect_RGB, ic->stringToIdentity(kinectRGB));
+        }
+        
+        avDepth = prop->getPropertyAsInt("KinectServer.CameraDepth");
+        if(avDepth){
+            std::string kinectDepth= "kinectDepth";
+            std::cout << "Creating Camera " + kinectDepth << std::endl;
+            kinect_Depth = new kinect::CamaraDepth(kinectDepth);
+            adapter->add(kinect_Depth, ic->stringToIdentity(kinectDepth));
+        }
+        
+        avCloud = prop->getPropertyAsInt("KinectServer.CloudPoints");
+        if(avCloud){
+            std::string kinectCloud = "kinect1";
+            std::cout << "Creating Camera " + kinectCloud << std::endl;
+            kinect_Cloud = new kinect::KinectI(kinectCloud);
+            adapter->add(kinect_Cloud, ic->stringToIdentity(kinectCloud));
+        }
+        
+        adapter->activate();   
+        
+        std::string s = prop->getPropertyWithDefault("KinectServer.LeafSize", "0.1");
+        leafSize = (float)atof(s.c_str());
+
+        cout << "leafSize: " << leafSize << endl;
+
+        int rc;
+        void *status;
+
+        pthread_t threads[NUM_THREADS];
+        rc =  pthread_create(&threads[0], NULL, &kinect::kinectThread, NULL);
+        if (rc){
+            printf("ERROR; return code from pthread_create() is %d\n", rc);
+            exit(-1);
+        }
+        rc = pthread_join(threads[0], &status);
+
+    
+    } catch (const Ice::Exception& ex) {
+        std::cerr <<  ex << std::endl;
+        exit(-1);
+    } catch (const char* msg) {
+        std::cerr << msg << std::endl;
+        exit(-1);
+    }
+/*
+    avCloud = prop->getPropertyAsInt("KinectServer.CloudPoints");
+    if(avCloud){
+        std::string kinectCloud = "kinect1";
+        std::cout << "Creating Camera " + kinectCloud << std::endl;
+        kinect_Cloud = new KinectI(kinectCloud, context());
+        context().createInterfaceWithString(kinect_Cloud, kinectCloud);
+    }
+*/
+
+}

Added: trunk/src/stable/components/pclRGBDServer/pointcloud.h
===================================================================
--- trunk/src/stable/components/pclRGBDServer/pointcloud.h	                        (rev 0)
+++ trunk/src/stable/components/pclRGBDServer/pointcloud.h	2013-10-14 15:41:26 UTC (rev 1048)
@@ -0,0 +1,69 @@
+
+namespace kinect{
+    pthread_mutex_t mutex;
+    pcl::PointCloud<pcl::PointXYZRGBA>cloud2;
+
+   class KinectI: virtual public jderobot::pointCloud{
+   public:
+		KinectI (std::string& propertyPrefix): prefix(propertyPrefix) {
+				
+			  KData = new jderobot::pointCloudData();
+
+              v=NULL;
+              v  = new CloudViewer(this);
+              v->start();
+			}
+		
+		virtual jderobot::pointCloudDataPtr getCloudData(const Ice::Current&){
+		      
+            pcl::PointCloud<pcl::PointXYZRGBA> cloud = v->getCloud();
+            std::cout << "Cloud size: " << cloud.points.size() << std::endl;
+
+            if(cloud.points.size()){
+               KData->p.resize(cloud.points.size());
+               int index = 0;
+               for(int i = 0; i < cloud.points.size(); i++){
+                  KData->p[index].x = cloud.points[i].x;
+                  KData->p[index].y = cloud.points[i].y;
+                  KData->p[index].z = cloud.points[i].z;
+                  KData->p[index].r = cloud.points[i].r;
+                  KData->p[index].g = cloud.points[i].g;
+                  KData->p[index].b = cloud.points[i].b;    
+                  index++;
+               }
+            }
+				return KData;
+		};
+
+
+	   
+	   private:
+	      class CloudViewer: public IceUtil::Thread{ 
+            public: 
+            CloudViewer (KinectI* kinect)
+             {}
+             
+            
+             virtual void run()
+             { 
+             }
+             
+             
+            pcl::PointCloud<pcl::PointXYZRGBA> getCloud()
+            {
+               pcl::PointCloud<pcl::PointXYZRGBA>cloud;
+               pthread_mutex_lock(&mutex);
+               cloud = cloud2;
+               pthread_mutex_unlock(&mutex);
+               return cloud;
+            }
+           
+         };
+         typedef IceUtil::Handle<CloudViewer> CloudViewerPtr;
+         CloudViewerPtr v; 
+    	 std::string prefix;
+         jderobot::pointCloudDataPtr KData;
+
+     }; //end class KinectI
+
+}



More information about the Jderobot-admin mailing list