#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <tf/transform_broadcaster.h>

class My_Filter
{
public:
    My_Filter();
    ~My_Filter(){}
    void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan);

private:
    ros::NodeHandle node_;
    laser_geometry::LaserProjection projector_;
    tf::TransformListener tfListener_;

    ros::Publisher point_cloud_publisher_;
    ros::Subscriber scan_sub_;
};

My_Filter::My_Filter()
{
    scan_sub_ = node_.subscribe<sensor_msgs::LaserScan>("/scan", 20, &My_Filter::scanCallback, this);
    point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 20, false);
    //tfListener_.setExtrapolationLimit(ros::Duration(0.1));
}

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
{
    sensor_msgs::PointCloud2 cloud;

    if (!tfListener_.waitForTransform(
            scan->header.frame_id,
            "/velodyne",
            scan->header.stamp + ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
            ros::Duration(1.0)))
    {
        return;
    }
    projector_.transformLaserScanToPointCloud("velodyne", *scan, cloud, tfListener_);
    point_cloud_publisher_.publish(cloud);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_filter");
    My_Filter filter;
    ros::Rate loop_rate(20);
    while (ros::ok())
    {
        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin(tf::Vector3(0, 0, 0));
        tf::Quaternion q;
        q.setRPY(0, 0, 0);
        transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "laser", "velodyne"));
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "laser_odom", "laser"));
        ros::spinOnce();
        loop_rate.sleep();
    }
    ros::spin();

    return 0;
}
