Fawkes API  Fawkes Development Version
pcl_frombuf_thread.cpp
00001 
00002 /***************************************************************************
00003  *  pcl_frombuf_thread.cpp - Create PCL point cloud from buffer
00004  *
00005  *  Created: Fri Dec 02 19:56:06 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "pcl_frombuf_thread.h"
00024 
00025 #include <fvutils/ipc/shm_image.h>
00026 #include <fvutils/color/colorspaces.h>
00027 #include <fvutils/base/types.h>
00028 #include <pcl_utils/utils.h>
00029 
00030 #include <memory>
00031 
00032 using namespace fawkes;
00033 using namespace firevision;
00034 
00035 /** @class OpenNiPclOnlyThread "pointcloud_thread.h"
00036  * OpenNI Point Cloud Provider Thread.
00037  * This thread provides a point cloud calculated from the depth image
00038  * acquired via OpenNI and provides it as a
00039  * SharedMemoryImageBuffer to other FireVision plugins.
00040  *
00041  * @author Tim Niemueller
00042  */
00043 
00044 /** Constructor. */
00045 OpenNiPclOnlyThread::OpenNiPclOnlyThread()
00046   : Thread("OpenNiPclOnlyThread", Thread::OPMODE_WAITFORWAKEUP),
00047     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
00048 {
00049 }
00050 
00051 
00052 /** Destructor. */
00053 OpenNiPclOnlyThread::~OpenNiPclOnlyThread()
00054 {
00055 }
00056 
00057 
00058 void
00059 OpenNiPclOnlyThread::init()
00060 {
00061   __pcl_buf = new SharedMemoryImageBuffer("openni-pointcloud");
00062 
00063 
00064   __width  = __pcl_buf->width();
00065   __height = __pcl_buf->height();
00066 
00067   __pcl = new pcl::PointCloud<pcl::PointXYZ>();
00068   __pcl->is_dense = false;
00069   __pcl->width    = __width;
00070   __pcl->height   = __height;
00071   __pcl->points.resize(__width * __height);
00072   __pcl->header.frame_id = "/kinect/depth";
00073 
00074   pcl_manager->add_pointcloud("openni-pointcloud", __pcl);
00075 }
00076 
00077 
00078 void
00079 OpenNiPclOnlyThread::finalize()
00080 {
00081   pcl_manager->remove_pointcloud("openni-pointcloud");
00082 
00083   delete __pcl_buf;
00084 }
00085 
00086 
00087 void
00088 OpenNiPclOnlyThread::loop()
00089 {
00090   if ((__pcl_buf->num_attached() > 1) || (__pcl.use_count() > 1))
00091   {
00092     __pcl_buf->lock_for_read();
00093     fawkes::Time capture_time = __pcl_buf->capture_time();
00094 
00095     if (__last_capture_time != capture_time) {
00096       __last_capture_time = capture_time;
00097 
00098       register pcl_point_t *pclbuf = (pcl_point_t *)__pcl_buf->buffer();
00099 
00100       pcl::PointCloud<pcl::PointXYZ> &pcl = **__pcl;
00101       pcl.header.seq += 1;
00102       pcl_utils::set_time(__pcl, capture_time);
00103 
00104       unsigned int idx = 0;
00105       for (unsigned int h = 0; h < __height; ++h) {
00106         for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
00107           // Fill in XYZ
00108           pcl.points[idx].x = pclbuf->x;
00109           pcl.points[idx].y = pclbuf->y;
00110           pcl.points[idx].z = pclbuf->z;
00111         }
00112       }
00113     }
00114 
00115     __pcl_buf->unlock();
00116   }
00117 }