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/tf_transform_cloud.h"
00037 #include <pluginlib/class_list_macros.h>
00038
00039 #include <tf2_ros/buffer_client.h>
00040 #include <pcl/common/centroid.h>
00041
00042 namespace jsk_pcl_ros_utils
00043 {
00044 void TfTransformCloud::transform(const sensor_msgs::PointCloud2ConstPtr &input)
00045 {
00046 vital_checker_->poke();
00047 sensor_msgs::PointCloud2 output;
00048 try
00049 {
00050 if (use_latest_tf_) {
00051 sensor_msgs::PointCloud2 latest_pointcloud(*input);
00052 latest_pointcloud.header.stamp = ros::Time(0);
00053 if (pcl_ros::transformPointCloud(target_frame_id_, latest_pointcloud, output,
00054 *tf_listener_)) {
00055 output.header.stamp = input->header.stamp;
00056 pub_cloud_.publish(output);
00057 }
00058 }
00059 else {
00060 if (pcl_ros::transformPointCloud(target_frame_id_, *input, output,
00061 *tf_listener_)) {
00062 pub_cloud_.publish(output);
00063 }
00064 }
00065 }
00066 catch (tf2::ConnectivityException &e)
00067 {
00068 NODELET_ERROR("Transform error: %s", e.what());
00069 }
00070 catch (tf2::InvalidArgumentException &e)
00071 {
00072 NODELET_ERROR("Transform error: %s", e.what());
00073 }
00074 catch (...)
00075 {
00076 NODELET_ERROR("Unknown transform error");
00077 }
00078 }
00079
00080 void TfTransformCloud::onInit(void)
00081 {
00082 DiagnosticNodelet::onInit();
00083
00084 if (!pnh_->getParam("target_frame_id", target_frame_id_))
00085 {
00086 ROS_WARN("~target_frame_id is not specified, using %s", "/base_footprint");
00087 }
00088 pnh_->param("duration", duration_, 1.0);
00089 pnh_->param("use_latest_tf", use_latest_tf_, false);
00090 pnh_->param("tf_queue_size", tf_queue_size_, 10);
00091 tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00092 pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00093
00094 onInitPostProcess();
00095 }
00096
00097 void TfTransformCloud::subscribe()
00098 {
00099 if (use_latest_tf_) {
00100 sub_cloud_ = pnh_->subscribe("input", 1, &TfTransformCloud::transform, this);
00101 }
00102 else {
00103 sub_cloud_message_filters_.subscribe(*pnh_, "input", 10);
00104 tf_filter_.reset(new tf::MessageFilter<sensor_msgs::PointCloud2>(sub_cloud_message_filters_,
00105 *tf_listener_,
00106 target_frame_id_,
00107 tf_queue_size_));
00108 tf_filter_->registerCallback(boost::bind(&TfTransformCloud::transform, this, _1));
00109 }
00110 }
00111
00112 void TfTransformCloud::unsubscribe()
00113 {
00114 if (use_latest_tf_) {
00115 sub_cloud_.shutdown();
00116 }
00117 else {
00118 sub_cloud_message_filters_.unsubscribe();
00119 }
00120 }
00121 }
00122
00123 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::TfTransformCloud, nodelet::Nodelet);