Fawkes API  Fawkes Development Version
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 = config->get_string("/plugins/openni/frame/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  pcl_point_t *pclbuf = (pcl_point_t *)__pcl_buf->buffer();
99 
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 }
fawkes::Time capture_time() const
Get the time when the image was captured.
Definition: shm_image.cpp:195
virtual ~OpenNiPclOnlyThread()
Destructor.
void remove_pointcloud(const char *id)
Remove the point cloud.
float y
Y value.
Definition: types.h:102
Definition: pointcloud.h:30
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void lock_for_read()
Lock shared memory segment for reading.
Definition: shm.cpp:867
float x
X value.
Definition: types.h:101
OpenNiPclOnlyThread()
Constructor.
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:235
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
virtual void finalize()
Finalize the thread.
int use_count() const
Get current reference count.
Definition: refptr.h:229
virtual void loop()
Code to execute in the thread.
Shared memory image buffer.
Definition: shm_image.h:181
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:714
unsigned int width() const
Get image width.
Definition: shm_image.cpp:255
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
Definition: types.h:100
virtual void init()
Initialize the thread.
float z
Z value.
Definition: types.h:103
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
unsigned int height() const
Get image height.
Definition: shm_image.cpp:265
void unlock()
Unlock memory.
Definition: shm.cpp:985
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.