39 #include <pcl/common/transforms.h> 41 #include <pcl/io/pcd_io.h> 42 #include <pcl/io/io.h> 48 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
49 DiagnosticNodelet::onInit();
51 pnh_->param(
"pcd_file", file_name, std::string(
""));
56 pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
70 const geometry_msgs::PoseStamped::ConstPtr& pose_stamped)
74 Eigen::Affine3f pose_eigen;
76 sensor_msgs::PointCloud2 ros_out;
77 Eigen::Matrix4f transform = pose_eigen.matrix();
79 ros_out.header = pose_stamped->header;
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_teacher_
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
sensor_msgs::PointCloud2 template_cloud_
virtual void poseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PCDReaderWithPose, nodelet::Nodelet)
#define NODELET_FATAL(...)
ros::Publisher pub_cloud_