How to turn laser scan to point cloud map

edit

One quick way to do this would be to use the laser_geometry node. It provides functionality for converting laser scans directly into PointCloud or PointCloud2 messages in the frame of your choice. That's what I use, and it's quite simple to do. I hope this helps.

EDIT: To use this functionality, you could do something like this:

In the class definition:

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

class My_Filter {
     public:
        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", 100, &My_Filter::scanCallback, this);
        point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("/cloud", 100, false);
        tfListener_.setExtrapolationLimit(ros::Duration(0.1));
}

In the callback function:

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 cloud;
    projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tfListener_);
    point_cloud_publisher_.publish(cloud);
}

And the main:

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_filter");

    My_Filter filter;

    ros::spin();

    return 0;
}

Where "base_link" is the frame that you'd like to see all the scans in, *scan is the reference to the incoming laser scan, cloud is the sensor_msgs::PointCloud2 that you'd like to be your output (this can also be a regular sensor_msgs::PointCloud), and tfListener_ is your transform listener.

You must also add the following to your manifest.xml:

<depend package="laser_geometry"/>
<depend package="tf"/>
<depend package="sensor_msgs" />

That's how I build my LaserScans into point clouds. I can provide a more specific example if you need it.

EDIT: I have updated the code snippets such that if you paste all of the code into a .cpp file, build the file, and run it, it will work 100%. You should change "/scan" to the topic that your laser publishes, and you should change "/cloud" to the topic you wish to view the cloud on. In Rviz, add a new PointCloud2 visualizer and point it to the topic you chose for "/cloud" and it should work. This node also assumes that there is a tf from the frame of the laser to the target frame ("base_link" in this example).