Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "jsk_pcl_ros_utils/pcd_reader_with_pose.h"
00037 #include "jsk_recognition_utils/pcl_conversion_util.h"
00038 #include <eigen_conversions/eigen_msg.h>
00039 #include <pcl/common/transforms.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/io/io.h>
00043
00044 namespace jsk_pcl_ros_utils
00045 {
00046 void PCDReaderWithPose::onInit()
00047 {
00048 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00049 DiagnosticNodelet::onInit();
00050 std::string file_name;
00051 pnh_->param("pcd_file", file_name, std::string(""));
00052 if (file_name == std::string("") || pcl::io::loadPCDFile (file_name, template_cloud_) == -1){
00053 NODELET_FATAL("cannot read pcd file %s", file_name.c_str());
00054 return;
00055 }
00056 pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00057
00058 onInitPostProcess();
00059 }
00060 void PCDReaderWithPose::subscribe()
00061 {
00062 sub_teacher_ = pnh_->subscribe("input", 1,
00063 &PCDReaderWithPose::poseCallback,
00064 this);
00065 }
00066 void PCDReaderWithPose::unsubscribe()
00067 {
00068 }
00069 void PCDReaderWithPose::poseCallback(
00070 const geometry_msgs::PoseStamped::ConstPtr& pose_stamped)
00071 {
00072 vital_checker_->poke();
00073 ros::Time now = ros::Time::now();
00074 Eigen::Affine3f pose_eigen;
00075 tf::poseMsgToEigen(pose_stamped->pose, pose_eigen);
00076 sensor_msgs::PointCloud2 ros_out;
00077 Eigen::Matrix4f transform = pose_eigen.matrix();
00078 pcl_ros::transformPointCloud(transform ,template_cloud_, ros_out);
00079 ros_out.header = pose_stamped->header;
00080 pub_cloud_.publish(ros_out);
00081 }
00082 }
00083
00084 #include <pluginlib/class_list_macros.h>
00085 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PCDReaderWithPose, nodelet::Nodelet);