Fawkes API
Fawkes Development Version
|
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 }