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
00037
00046
00047 #include <ros/ros.h>
00048
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051 #include "pcl_ros/transforms.h"
00052
00053
00054
00055
00056 #include <tf/transform_listener.h>
00057
00058 using namespace std;
00059
00060 class TransformPointcloudNode
00061 {
00062 protected:
00063 ros::NodeHandle nh_;
00064
00065 public:
00066 string output_cloud_topic_, input_cloud_topic_, to_frame_, point_cloud_name_;
00067 bool save_point_cloud_;
00068
00069 ros::Subscriber sub_;
00070 ros::Publisher pub_;
00071 tf::TransformListener tf_;
00072 pcl::PCDWriter pcd_writer_;
00073 sensor_msgs::PointCloud2 output_cloud_;
00075 TransformPointcloudNode (ros::NodeHandle &n) : nh_(n)
00076 {
00077
00078 nh_.param("input_cloud_topic", input_cloud_topic_, std::string("cloud_pcd"));
00079 output_cloud_topic_ = input_cloud_topic_ + "_transformed";
00080 nh_.param("to_frame", to_frame_, std::string("base_link"));
00081 nh_.param("point_cloud_name", point_cloud_name_, std::string("pc"));
00082 nh_.param("save_point_cloud", save_point_cloud_, false);
00083
00084 sub_ = nh_.subscribe (input_cloud_topic_, 1, &TransformPointcloudNode::cloud_cb, this);
00085 ROS_INFO ("[TransformPointcloudNode:] Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00086 pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_topic_, 1);
00087 ROS_INFO ("[TransformPointcloudNode:] Will be publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00088 }
00089
00091
00092 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00093 {
00094 bool found_transform = tf_.waitForTransform(pc->header.frame_id, to_frame_,
00095 pc->header.stamp, ros::Duration(10.0));
00096 if (found_transform)
00097 {
00098
00099 tf::StampedTransform transform;
00100 tf_.lookupTransform(to_frame_,pc->header.frame_id, pc->header.stamp, transform);
00101 pcl_ros::transformPointCloud(to_frame_, transform, *pc, output_cloud_);
00102 ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", output_cloud_.header.frame_id.c_str());
00103 pub_.publish (output_cloud_);
00104 if (save_point_cloud_)
00105 {
00106 pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
00107 pcl::fromROSMsg(output_cloud_, pcl_cloud);
00108 pcd_writer_.write (point_cloud_name_ + ".pcd", pcl_cloud);
00109 ROS_INFO("Point cloud saved as %s.pcd", point_cloud_name_.c_str());
00110 }
00111 }
00112 }
00113 };
00114
00115 int main (int argc, char** argv)
00116 {
00117 ros::init (argc, argv, "transform_pointcloud_node");
00118 ros::NodeHandle n("~");
00119 TransformPointcloudNode tp(n);
00120 ros::spin ();
00121 return (0);
00122 }
00123
00124