23 #include "pcl_frombuf_thread.h"
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>
46 :
Thread(
"OpenNiPclOnlyThread",
Thread::OPMODE_WAITFORWAKEUP),
64 __width = __pcl_buf->
width();
65 __height = __pcl_buf->
height();
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";
95 if (__last_capture_time != capture_time) {
96 __last_capture_time = capture_time;
102 pcl_utils::set_time(__pcl, capture_time);
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) {
108 pcl.points[idx].x = pclbuf->
x;
109 pcl.points[idx].y = pclbuf->
y;
110 pcl.points[idx].z = pclbuf->
z;
virtual ~OpenNiPclOnlyThread()
Destructor.
void remove_pointcloud(const char *id)
Remove the point cloud.
Fawkes library namespace.
unsigned int height() const
Get image height.
A class for handling time.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
void lock_for_read()
Lock shared memory segment for reading.
OpenNiPclOnlyThread()
Constructor.
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
Shared memory image buffer.
unsigned char * buffer() const
Get image buffer.
unsigned int width() const
Get image width.
unsigned int num_attached() const
Get number of attached processes.
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
virtual void init()
Initialize the thread.
int use_count() const
Get current reference count.
void unlock()
Unlock memory.
fawkes::Time capture_time() const
Get the time when the image was captured.