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/base/types.h>
26 #include <fvutils/color/colorspaces.h>
27 #include <fvutils/ipc/shm_image.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 /** Destructor. */
53 {
54 }
55 
56 void
58 {
59  pcl_buf_ = new SharedMemoryImageBuffer("openni-pointcloud");
60 
61  width_ = pcl_buf_->width();
62  height_ = pcl_buf_->height();
63 
64  pcl_ = new pcl::PointCloud<pcl::PointXYZ>();
65  pcl_->is_dense = false;
66  pcl_->width = width_;
67  pcl_->height = height_;
68  pcl_->points.resize((size_t)width_ * (size_t)height_);
69  pcl_->header.frame_id = config->get_string("/plugins/openni/frame/depth");
70 
71  pcl_manager->add_pointcloud("openni-pointcloud", pcl_);
72 }
73 
74 void
76 {
77  pcl_manager->remove_pointcloud("openni-pointcloud");
78 
79  delete pcl_buf_;
80 }
81 
82 void
84 {
85  if ((pcl_buf_->num_attached() > 1) || (pcl_.use_count() > 1)) {
86  pcl_buf_->lock_for_read();
87  fawkes::Time capture_time = pcl_buf_->capture_time();
88 
89  if (last_capture_time_ != capture_time) {
90  last_capture_time_ = capture_time;
91 
92  pcl_point_t *pclbuf = (pcl_point_t *)pcl_buf_->buffer();
93 
94  pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_;
95  pcl.header.seq += 1;
96  pcl_utils::set_time(pcl_, capture_time);
97 
98  unsigned int idx = 0;
99  for (unsigned int h = 0; h < height_; ++h) {
100  for (unsigned int w = 0; w < width_; ++w, ++idx, ++pclbuf) {
101  // Fill in XYZ
102  pcl.points[idx].x = pclbuf->x;
103  pcl.points[idx].y = pclbuf->y;
104  pcl.points[idx].z = pclbuf->z;
105  }
106  }
107  }
108 
109  pcl_buf_->unlock();
110  }
111 }
virtual void finalize()
Finalize the thread.
virtual ~OpenNiPclOnlyThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
OpenNiPclOnlyThread()
Constructor.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
void remove_pointcloud(const char *id)
Remove the point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
int use_count() const
Get current reference count.
Definition: refptr.h:235
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:763
void unlock()
Unlock memory.
Definition: shm.cpp:1025
void lock_for_read()
Lock shared memory segment for reading.
Definition: shm.cpp:909
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Shared memory image buffer.
Definition: shm_image.h:184
unsigned int height() const
Get image height.
Definition: shm_image.cpp:255
fawkes::Time capture_time() const
Get the time when the image was captured.
Definition: shm_image.cpp:189
unsigned int width() const
Get image width.
Definition: shm_image.cpp:246
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
Fawkes library namespace.
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
Definition: types.h:98
float z
Z value.
Definition: types.h:101
float x
X value.
Definition: types.h:99
float y
Y value.
Definition: types.h:100