Laser Scan To Point Cloud Transform

/**
  * Script to transform sensor_msgs::LaserScan data
  * from Lidar to sensor_msgs::PointCloud data.
  * @Author Sonia Mannan
  * @Version 4/25/2015
 */

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/PointCloud.h"
#include "laser_geometry/laser_geometry.h"
#include <tf/transform_listener.h>

//create a LaserProjection object
//to project laserScan data to point_cloud
laser_geometry::LaserProjection projector;

//create a Transform Listener object to
//receive transforms
tf::TransformListener listen_scan;

//create a PointCloud object to project data onto.
sensor_msgs::PointCloud cloud;

/**
  * Callback to process LaserScan data from Lidar.
  * @param scan_input a sensor_msgs::LaserScan pointer
  * i.e. pointer to LaserScan data from Lidar
  * Will take in LaserScan data, transform using a
  * TransformListener and project data using LaserProjection
  * to a point cloud.
*/
void Process_Lidar_Data(const sensor_msgs::LaserScan::ConstPtr& scan_input)
{
   /**
     * Wait until a Transform becomes available.
     *
     * First param: frame to transform from
     * scan_input->header.frame_id = frame of the
     * current LaserScan input
     *
          *
     * First param: frame to transform from
     * scan_input->header.frame_id = frame of the
     * current LaserScan input
     *
     * Second param: frame to transform to
     * "/base_link" = frame to transform to
     *
     * Third param: when to transform
     * scan_input->header.stamp = time first frame is received
     * ros::Duration().fromSec = ros Duration is a time
     * period, fromSec is a function to convert from Seconds
     * scan_input->ranges.size() = size of data array received
     * scan_input->time_increment = time between measurements
     * in seconds.
     * Transforms at time = (when the first frame is received +
     * data size*time between measurements) seconds
     *
     * Fourth param: when to time out
     * ros::Duration(1.0) = 1 second
     * Do not wait longer than 1 second.
   */

   if(!listen_scan.waitForTransform
         (scan_input->header.frame_id, "/base_link",
          scan_input->header.stamp+ros::Duration().
          fromSec(scan_input->ranges.size()*scan_input
          ->time_increment),ros::Duration(1.0)))
   {

         return;
   }

   //Transform LaserScan to PointCloud data
   projector.transformLaserScanToPointCloud("/base_link", *scan_input,
      cloud, listen_scan);
}

void printPointCloudData(sensor_msgs::PointCloud cloud)
{
   for(unsigned int i = 0; i < cloud.points.size(); i++)
   {
      ROS_INFO("X Coord: %f", cloud.points[i].x);
      ROS_INFO("Y Coord: %f", cloud.points[i].y);
      ROS_INFO("Z Coord: %f", cloud.points[i].z);
   }
}

int main (int argc, char **argv)
{
   //Initialize the ROS lidar listener
   //Must be called before using ROS System

   ros::init(argc, argv, "lidar_listener");

   //Create ROS NodeHandle to communicate with
   //the ROS System

   ros::NodeHandle n_lidar;

   //Subscribe to _scan topic (broadcasts Lidar data)
   //Tells ROS you want to receive messages from Lidar
   //Messages passed to Proess_Lidar_Data callback (above)

   ros::Subscriber lidar_sub = n_lidar.subscribe("_scan",
      1000, Process_Lidar_Data);

   printPointCloudData(cloud);

   ros::spin();

   return 0;
}