Reading PointCloud2 in C++

edit

You seem already got an accepted answer but just for future reference,

You could use convertPointCloud2ToPointCloud from sensor_msgs.

Once you input Pointcloud2 and got output as Pointcloud , then you can iterate through your points as such;

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::Pointcloud2 input_pointcloud;
sensor_msgs::Pointcloud out_pointcloud;
sensor_msgs::convertPointCloud2ToPointCloud(input_pointcloud, out_cloud);

for(int i = 0 ; i < out_cloud.points.size(); ++i){
    geometry_msgs::Point32 point;

    //Dooo something here
    point.z = out_cloud.points[i].z;
}

And get xyz coordinates of all points