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 #define BOOST_PARAMETER_MAX_ARITY 7
00037
00038 #include "jsk_pcl_ros_utils/pointcloud_relative_from_pose_stamped.h"
00039 #include <jsk_recognition_utils/tf_listener_singleton.h>
00040 #include <jsk_recognition_utils/pcl_conversion_util.h>
00041 #include <jsk_recognition_utils/pcl_ros_util.h>
00042 #include <pcl/common/transforms.h>
00043
00044 namespace jsk_pcl_ros_utils
00045 {
00046 void PointCloudRelativeFromPoseStamped::onInit()
00047 {
00048 DiagnosticNodelet::onInit();
00049 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00050 onInitPostProcess();
00051 }
00052
00053 void PointCloudRelativeFromPoseStamped::subscribe()
00054 {
00055 sub_cloud_.subscribe(*pnh_, "input", 1);
00056 sub_pose_.subscribe(*pnh_, "input/pose", 1);
00057 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00058 sync_->connectInput(sub_cloud_, sub_pose_);
00059 sync_->registerCallback(boost::bind(&PointCloudRelativeFromPoseStamped::transform, this, _1, _2));
00060 }
00061
00062 void PointCloudRelativeFromPoseStamped::unsubscribe()
00063 {
00064 sub_cloud_.unsubscribe();
00065 sub_pose_.unsubscribe();
00066 }
00067
00068 void PointCloudRelativeFromPoseStamped::transform(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00069 const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00070 {
00071 vital_checker_->poke();
00072 if (!jsk_recognition_utils::isSameFrameId(cloud_msg->header.frame_id,
00073 pose_msg->header.frame_id)) {
00074 NODELET_ERROR("frame_id does not match. cloud: %s, pose: %s",
00075 cloud_msg->header.frame_id.c_str(),
00076 pose_msg->header.frame_id.c_str());
00077 return;
00078 }
00079 Eigen::Affine3f transform;
00080 tf::poseMsgToEigen(pose_msg->pose, transform);
00081 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00082 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
00083 pcl::fromROSMsg(*cloud_msg, *cloud);
00084 Eigen::Affine3f inverse_transform = transform.inverse();
00085 pcl::transformPointCloud(*cloud, *transformed_cloud, inverse_transform);
00086 sensor_msgs::PointCloud2 ros_cloud;
00087 pcl::toROSMsg(*transformed_cloud, ros_cloud);
00088 ros_cloud.header = cloud_msg->header;
00089 pub_.publish(ros_cloud);
00090 }
00091 }
00092
00093 #include <pluginlib/class_list_macros.h>
00094 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PointCloudRelativeFromPoseStamped, nodelet::Nodelet);