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

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.

score 0 · Answer 1 · answered Sep 24 '21 at 16:58

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.

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

1 Answers1