diff --git a/io/include/pcl/io/pcd_grabber.h b/io/include/pcl/io/pcd_grabber.h index def8422770b..38f2633f951 100644 --- a/io/include/pcl/io/pcd_grabber.h +++ b/io/include/pcl/io/pcd_grabber.h @@ -144,7 +144,7 @@ namespace pcl private: virtual void - publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0; + publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const = 0; // to separate and hide the implementation from interface: PIMPL struct PCDGrabberImpl; @@ -175,9 +175,10 @@ namespace pcl protected: virtual void - publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const; + publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const; boost::signals2::signal >&)>* signal_; + boost::signals2::signal* file_name_signal_; #ifdef HAVE_OPENNI boost::signals2::signal&)>* depth_image_signal_; @@ -192,6 +193,7 @@ namespace pcl : PCDGrabberBase (pcd_path, frames_per_second, repeat) { signal_ = createSignal >&)>(); + file_name_signal_ = createSignal(); #ifdef HAVE_OPENNI depth_image_signal_ = createSignal &)> (); image_signal_ = createSignal &)> (); @@ -205,6 +207,7 @@ namespace pcl : PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ () { signal_ = createSignal >&)>(); + file_name_signal_ = createSignal(); #ifdef HAVE_OPENNI depth_image_signal_ = createSignal &)> (); image_signal_ = createSignal &)> (); @@ -236,7 +239,7 @@ namespace pcl //////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void - PCDGrabber::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const + PCDGrabber::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const { typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); pcl::fromPCLPointCloud2 (blob, *cloud); @@ -244,6 +247,8 @@ namespace pcl cloud->sensor_orientation_ = orientation; signal_->operator () (cloud); + if (file_name_signal_->num_slots() > 0) + file_name_signal_->operator()(file_name); #ifdef HAVE_OPENNI // If dataset is not organized, return diff --git a/io/src/pcd_grabber.cpp b/io/src/pcd_grabber.cpp index c1e7e2facc0..1ee3a727cb1 100644 --- a/io/src/pcd_grabber.cpp +++ b/io/src/pcd_grabber.cpp @@ -95,6 +95,7 @@ struct pcl::PCDGrabberBase::PCDGrabberImpl pcl::PCLPointCloud2 next_cloud_; Eigen::Vector4f origin_; Eigen::Quaternionf orientation_; + std::string next_file_name_; bool valid_; // TAR reading I/O @@ -127,6 +128,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::PCDGrabberImpl (pcl::PCDGrabberBase& grabbe , next_cloud_ () , origin_ () , orientation_ () + , next_file_name_ () , valid_ (false) , tar_fd_ (-1) , tar_offset_ (0) @@ -136,6 +138,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::PCDGrabberImpl (pcl::PCDGrabberBase& grabbe { pcd_files_.push_back (pcd_path); pcd_iterator_ = pcd_files_.begin (); + next_file_name_ = *pcd_iterator_; readAhead (); } @@ -151,6 +154,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::PCDGrabberImpl (pcl::PCDGrabberBase& grabbe , next_cloud_ () , origin_ () , orientation_ () + , next_file_name_ () , valid_ (false) , tar_fd_ (-1) , tar_offset_ (0) @@ -160,6 +164,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::PCDGrabberImpl (pcl::PCDGrabberBase& grabbe { pcd_files_ = pcd_files; pcd_iterator_ = pcd_files_.begin (); + next_file_name_ = *pcd_iterator_; readAhead (); } @@ -210,6 +215,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::readAhead () } } + next_file_name_ = *pcd_iterator_; if (++pcd_iterator_ == pcd_files_.end () && repeat_) pcd_iterator_ = pcd_files_.begin (); } @@ -288,7 +294,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::trigger () { boost::mutex::scoped_lock read_ahead_lock(read_ahead_mutex_); if (valid_) - grabber_.publish (next_cloud_,origin_,orientation_); + grabber_.publish (next_cloud_,origin_,orientation_, next_file_name_); // use remaining time, if there is time left! readAhead ();