transform_pointcloud.cpp

Go to the documentation of this file.

// ROS core
#include <ros/ros.h>
#include <tf/transform_listener.h>
// PCL includes
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>


using namespace std;

class TransformPointCloud
{
  protected:
    ros::NodeHandle nh_;

  public:
    string cloud_topic_sub_;
    string cloud_topic_pub_;
    string target_frame_;

    tf::TransformListener tf_listener_;
    ros::Subscriber sub_;
    ros::Publisher pub_;

    TransformPointCloud() :
      nh_("~")
    {
      cloud_topic_sub_="input";
      cloud_topic_pub_="output";
      target_frame_ = "/map";

      pub_ = nh_.advertise<sensor_msgs::PointCloud2>(cloud_topic_pub_,1);
      sub_ = nh_.subscribe (cloud_topic_sub_, 1,  &TransformPointCloud::cloud_cb, this);
    }

    // Callback
    void
      cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
    {
      sensor_msgs::PointCloud2 cloud_out;
      pcl_ros::transformPointCloud (target_frame_, *cloud, cloud_out, tf_listener_);
      cloud_out.header.stamp = cloud->header.stamp;
      pub_.publish(cloud_out);
    }

};

/* ---[ */
int
  main (int argc, char** argv)
{
  ros::init (argc, argv, "transform_pointcloud", ros::init_options::AnonymousName);

  TransformPointCloud b;
  ros::spin ();

  return (0);
}
/* ]--- */