How to read the x,y,z coordinates of point cloud from the LIDAR SICK LMS 200.

edit

Goal: To employ obstacle avoidance by interpreting the data from the LIDAR using PCL

Description: Hi everyone, I am working with a LIDAR device for the first time. I used the sick toolbox ros wrapper for reading the values from the LIDAR. I converted the "scan" topic given out by the LIDAR device to point-cloud 2 format. here is the program:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/point_cloud.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>

 ros::Publisher pub;

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));
}

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

    projector_.transformLaserScanToPointCloud("/laser", *scan, cloud, tfListener_);
    point_cloud_publisher_.publish(cloud);
}



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

    My_Filter filter;

    ros::spin();

    return 0;
}

This above node gives out the point cloud 2 topic "cloud". I subscribed to this topic in the following program. I was successful in reading the widht and height of the point clouds,but I need some help reading the X,Y,Z coordinates. I also need to know how to access the other objects which might be useful in data processing.

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/point_cloud.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/ros/conversions.h> 
ros::Publisher pub;
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // ... do data processing
 printf ("Cloud: width = %d, height = %d\n", input->width,input->height);

  BOOST_FOREACH (const pcl::PointXYZ& pt, input->points)
  printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
  // Publish the data
 // pub.publish (output);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "pcs200");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/cloud", 100, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 100,false);

  // Spin
  ros::spin ();
}

Here is the output for that:

[  0%] Built target rospack_genmsg_libexe
  [  0%] Built target rosbuild_precompile
  Scanning dependencies of target pcs200
  [100%] Building CXX object CMakeFiles/pcs200.dir/src/pcs200.o
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp: In function ‘void cloud_cb(const sensor_msgs::PointCloud2ConstPtr&)’:
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp:17: error: ‘const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has no member named ‘points’
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp:17: error: ‘const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has no member named ‘points’
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp:17: error: ‘const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has ...