Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
pcl_frombuf_thread.cpp
1 
2 /***************************************************************************
3  * pcl_frombuf_thread.cpp - Create PCL point cloud from buffer
4  *
5  * Created: Fri Dec 02 19:56:06 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "pcl_frombuf_thread.h"
24 
25 #include <fvutils/ipc/shm_image.h>
26 #include <fvutils/color/colorspaces.h>
27 #include <fvutils/base/types.h>
28 #include <pcl_utils/utils.h>
29 
30 #include <memory>
31 
32 using namespace fawkes;
33 using namespace firevision;
34 
35 /** @class OpenNiPclOnlyThread "pointcloud_thread.h"
36  * OpenNI Point Cloud Provider Thread.
37  * This thread provides a point cloud calculated from the depth image
38  * acquired via OpenNI and provides it as a
39  * SharedMemoryImageBuffer to other FireVision plugins.
40  *
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor. */
46  : Thread("OpenNiPclOnlyThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
48 {
49 }
50 
51 
52 /** Destructor. */
54 {
55 }
56 
57 
58 void
60 {
61  __pcl_buf = new SharedMemoryImageBuffer("openni-pointcloud");
62 
63 
64  __width = __pcl_buf->width();
65  __height = __pcl_buf->height();
66 
67  __pcl = new pcl::PointCloud<pcl::PointXYZ>();
68  __pcl->is_dense = false;
69  __pcl->width = __width;
70  __pcl->height = __height;
71  __pcl->points.resize(__width * __height);
72  __pcl->header.frame_id = "/kinect/depth";
73 
74  pcl_manager->add_pointcloud("openni-pointcloud", __pcl);
75 }
76 
77 
78 void
80 {
81  pcl_manager->remove_pointcloud("openni-pointcloud");
82 
83  delete __pcl_buf;
84 }
85 
86 
87 void
89 {
90  if ((__pcl_buf->num_attached() > 1) || (__pcl.use_count() > 1))
91  {
92  __pcl_buf->lock_for_read();
93  fawkes::Time capture_time = __pcl_buf->capture_time();
94 
95  if (__last_capture_time != capture_time) {
96  __last_capture_time = capture_time;
97 
98  register pcl_point_t *pclbuf = (pcl_point_t *)__pcl_buf->buffer();
99 
100  pcl::PointCloud<pcl::PointXYZ> &pcl = **__pcl;
101  pcl.header.seq += 1;
102  pcl_utils::set_time(__pcl, capture_time);
103 
104  unsigned int idx = 0;
105  for (unsigned int h = 0; h < __height; ++h) {
106  for (unsigned int w = 0; w < __width; ++w, ++idx, ++pclbuf) {
107  // Fill in XYZ
108  pcl.points[idx].x = pclbuf->x;
109  pcl.points[idx].y = pclbuf->y;
110  pcl.points[idx].z = pclbuf->z;
111  }
112  }
113  }
114 
115  __pcl_buf->unlock();
116  }
117 }