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
32using namespace fawkes;
33using 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
56void
58{
59 pcl_buf_ = new SharedMemoryImageBuffer("openni-pointcloud");
60
61 width_ = pcl_buf_->width();
62 height_ = pcl_buf_->height();
63
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
74void
76{
77 pcl_manager->remove_pointcloud("openni-pointcloud");
78
79 delete pcl_buf_;
80}
81
82void
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
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