40 #include <pcl/pcl_config.h>
42 #include <pcl/common/io.h>
43 #include <pcl/io/grabber.h>
44 #include <pcl/io/file_grabber.h>
45 #include <pcl/common/time_trigger.h>
46 #include <pcl/conversions.h>
47 #include <pcl/memory.h>
50 #include <pcl/io/openni_camera/openni_image.h>
51 #include <pcl/io/openni_camera/openni_image_rgb24.h>
52 #include <pcl/io/openni_camera/openni_depth_image.h>
71 PCDGrabberBase (
const std::string& pcd_file,
float frames_per_second,
bool repeat);
78 PCDGrabberBase (
const std::vector<std::string>& pcd_files,
float frames_per_second,
bool repeat);
99 isRunning ()
const override;
103 getName ()
const override;
111 getFramesPerSecond ()
const override;
119 getCloudAt (std::size_t idx,
121 Eigen::Vector4f &origin,
122 Eigen::Quaternionf &orientation)
const;
130 publish (
const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation,
const std::string& file_name)
const = 0;
133 struct PCDGrabberImpl;
134 PCDGrabberImpl* impl_;
142 using Ptr = shared_ptr<PCDGrabber>;
145 PCDGrabber (
const std::string& pcd_path,
float frames_per_second = 0,
bool repeat =
false);
146 PCDGrabber (
const std::vector<std::string>& pcd_files,
float frames_per_second = 0,
bool repeat =
false);
160 size ()
const override;
164 publish (
const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation,
const std::string& file_name)
const override;
166 boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>*
signal_;
171 boost::signals2::signal<void (const openni_wrapper::Image::Ptr&)>*
image_signal_;
172 boost::signals2::signal<void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)>*
image_depth_image_signal_;
177 template<
typename Po
intT>
181 signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
185 image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
186 image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
191 template<
typename Po
intT>
193 :
PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
195 signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
199 image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
200 image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
209 Eigen::Vector4f origin;
210 Eigen::Quaternionf orientation;
211 getCloudAt (idx, blob, origin, orientation);
220 template <
typename Po
intT> std::size_t
223 return (numFrames ());
227 template<
typename Po
intT>
void
235 signal_->operator () (cloud);
236 if (file_name_signal_->num_slots() > 0)
237 file_name_signal_->operator()(file_name);
244 shared_ptr<xn::DepthMetaData> depth_meta_data (
new xn::DepthMetaData);
245 depth_meta_data->AllocateData (cloud->
width, cloud->
height);
246 XnDepthPixel* depth_map = depth_meta_data->WritableData ();
248 for (std::uint32_t i = 0; i < cloud->
height; ++i)
249 for (std::uint32_t j = 0; j < cloud->
width; ++j)
251 depth_map[k] =
static_cast<XnDepthPixel
> ((*cloud)[k].z * 1000);
256 if (depth_image_signal_->num_slots() > 0)
257 depth_image_signal_->operator()(depth_image);
260 std::vector<pcl::PCLPointField> fields;
261 int rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
262 if (rgba_index == -1)
263 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
266 rgba_index = fields[rgba_index].offset;
268 shared_ptr<xn::ImageMetaData> image_meta_data (
new xn::ImageMetaData);
269 image_meta_data->AllocateData (cloud->
width, cloud->
height, XN_PIXEL_FORMAT_RGB24);
270 XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data ();
272 for (std::uint32_t i = 0; i < cloud->
height; ++i)
274 for (std::uint32_t j = 0; j < cloud->
width; ++j)
278 memcpy (&rgb, reinterpret_cast<const char*> (&cloud->
points[k]) + rgba_index, sizeof (
RGB));
279 image_map[k].nRed =
static_cast<XnUInt8
> (rgb.r);
280 image_map[k].nGreen =
static_cast<XnUInt8
> (rgb.g);
281 image_map[k].nBlue =
static_cast<XnUInt8
> (rgb.b);
287 if (image_signal_->num_slots() > 0)
288 image_signal_->operator()(image);
290 if (image_depth_image_signal_->num_slots() > 0)
291 image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f);
shared_ptr< PCDGrabber > Ptr
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
boost::signals2::signal< void(const openni_wrapper::DepthImage::Ptr &)> * depth_image_signal_
boost::signals2::signal< void(const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float constant)> * image_depth_image_signal_
boost::signals2::signal< void(const openni_wrapper::Image::Ptr &)> * image_signal_
boost::signals2::signal< void(const typename pcl::PointCloud< PointT >::ConstPtr &)> * signal_
~PCDGrabber() noexcept
Virtual destructor.
void stop() override
Stops playing the list of PCD files if frames_per_second is > 0.
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Grabber interface for PCL 1.x device drivers.
PCDGrabber(const std::string &pcd_path, float frames_per_second=0, bool repeat=false)
std::size_t size() const override
size Returns the number of clouds currently loaded by the grabber
This class provides methods to fill a depth or disparity image.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
A structure representing RGB color information.
void publish(const pcl::PCLPointCloud2 &blob, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, const std::string &file_name) const override
shared_ptr< PointCloud< PointT > > Ptr
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
const pcl::PointCloud< PointT >::ConstPtr operator[](std::size_t idx) const override
operator[] Returns the idx-th cloud in the dataset, without bounds checking.
FileGrabber provides a container-style interface for grabbers which operate on fixed-size input...
Base class for PCD file grabber.
This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::shared_ptr< Image > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
pcl::shared_ptr< DepthImage > Ptr
boost::signals2::signal< void(const std::string &)> * file_name_signal_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< const PCDGrabber > ConstPtr