Writing / Accessing points in a PointCloud2 structure C++ / ROS / PCL Library - c++

Question

I am currently building a perception pipeline and am having trouble reading the points of my point cloud structure. I am reading in a point cloud into a PointCloud2 Structure and want to be able to write out the points in series of the point cloud or at the very least access them so I find a way to write them to a file later. The basic code I am working with is here:
void cloud_cropbox_cb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// convert given message into PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
I want to be able to access the x,y,z elements of each point in the cloud and ideally just write them all out to a textfile. I have tried using pcl::io::savePCDFileASCII() but I don't think that is quite applicable here. An additional note is the base program is constantly running and never exits until manually closed I don't know if that will be relavent to writing out to a file. Any help on what exact functions to use / steps I need to take would be much appreciated.

Answer 1

If you literally want to write out the contents of the message to a file, you can just (in launch file or commandline) use ros topic echo [args] > my_file.txt.
For ways that are compatible in C++, I recommend reading from http://wiki.ros.org/pcl_ros, which describes that:
pcl_ros extends the ROS C++ client library to support message passing with PCL native data types. Simply add the following include to your ROS node source code:
#include <pcl_ros/point_cloud.h>
This header allows you to publish and subscribe pcl::PointCloud objects as ROS messages. These appear to ROS as sensor_msgs/PointCloud2 messages, offering seamless interoperability with non-PCL-using ROS nodes. For example, you may publish a pcl::PointCloud in one of your nodes and visualize it in rviz using a Point Cloud2 display. It works by hooking into the roscpp serialization infrastructure.
This means that you can internally use PCL all you want, and it's all a ROS PointCloud2 message. But you have to declare the PCL type / interface you want to use in the message / publisher / subscriber that you access the data with. So instead of using the sensor_msgs/PointCloud2 type everywhere, use the pcl::Pointcloud<pcl::Point*> type everywhere.
Publishing:
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1);
PointCloud::Ptr msg (new PointCloud);
msg->points.push_back (pcl::PointXYZ(1.0, 2.0, 3.0));
pub.publish (msg);
Subscribing:
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
void callback(const PointCloud::ConstPtr& msg){
printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
}
More examples (or links to node source code) in http://wiki.ros.org/pcl_ros, tutorials on other pipeline stuff in http://wiki.ros.org/pcl_ros/Tutorials.