39 #include <pcl/io/pcd_io.h> 40 #include <pcl/io/vtk_lib_io.h> 49 cloud_.reset(
new pcl::PointCloud<pcl::PointXYZRGB>);
51 ext_ = path.extension().string();
52 stem_ = path.stem().string();
69 sensor_msgs::PointCloud2
72 sensor_msgs::PointCloud2 ros_out;
74 ros_out.header.stamp =
stamp;
75 ros_out.header.frame_id =
stem_;
87 pub_points_array_ = pnh_->advertise<jsk_recognition_msgs::PointsArray>(
"output", 1);
88 pub_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>(
"cloud", 1);
90 || files_.size() == 0) {
95 for (
size_t i = 0; i< files_.size(); i++) {
97 point_clouds_.push_back(data);
100 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
101 dynamic_reconfigure::Server<Config>::CallbackType
f =
104 srv_->setCallback (f);
105 pnh_->getParam(
"duration", duration_);
111 for (
size_t i = 0; i< files_.size(); i++) {
112 array_msg_.cloud_list[i].header.stamp =
event.current_real;
114 array_msg_.header.stamp =
event.current_real;
115 pub_points_array_.publish(array_msg_);
117 point_msg_ = point_clouds_[0]->getROSPointCloud(event.
current_real);
118 point_msg_.header.stamp =
event.current_real;
119 pub_cloud_.publish(point_msg_);
126 duration_ = config.duration;
127 use_array_ = config.use_array;
const std::string file_name_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PointcloudDatabaseServer, nodelet::Nodelet)
virtual ~PointcloudDatabaseServer()
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getPointCloud()
virtual void timerCallback(const ros::TimerEvent &event)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
virtual sensor_msgs::PointCloud2 getROSPointCloud(ros::Time stamp)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
jsk_pcl_ros::PointcloudDatabaseServerConfig Config
PointCloudData(const std::string fname)
#define NODELET_FATAL(...)
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud_
virtual void configCallback(Config &config, uint32_t level)