sending PCL binary stream

asked 2020-06-09 12:09:21 -0500

toml gravatar image

I am trying to send the binary stream produced by the PCL library over ROS. I use the octree compression function to compress lidar data. This function returns a binary output stream. In their tutorial, they place it in a stringstream, so I did the same. Because you cannot publish a stringstream, I convert it to a string and publish this to the ROS network.

At the decompression side, I try to convert the string back to binary and call the decompression function. When I run the compression node, everything works fine. I can see the message getting published. When I run the decompression node, it throws an std::bad_alloc error.

Does someone as experience with sending a binary output stream with ROS or can tell why I get the bad allocation error?

I run the following programs on a VM with Ubuntu 18.04 and ROS melodic.

Decompression node:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/compression/octree_pointcloud_compression.h>
#include <stdio.h>
#include <sstream>
#include <stdlib.h>
#include <chrono>

#include <string>
#include <std_msgs/String.h>

ros::Publisher pub;

pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;

void 
cloud_cb (const std_msgs::String& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 output;
  // stringstream to store compressed point cloud
  std::stringstream compressedData;

    compressedData << input.data;

  // output pointcloud

  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());

  // decompress point cloud
  PointCloudDecoder->decodePointCloud (compressedData, cloudOut);


  //convert to pointcloud2
  pcl::toROSMsg(*cloudOut, output);

  // Publish the data.
  pub.publish (output);
}

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

  //\\
  //setup compression algorithm

  PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();
  //\\

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

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

  // Spin
  ros::spin ();
}

Compression node:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

#include <pcl/compression/octree_pointcloud_compression.h>

#include <stdio.h>
#include <sstream>
#include <stdlib.h>
#include <chrono>
#include <string>
#include <std_msgs/String.h>

ros::Publisher pub;

pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  std::string output;
  std_msgs::String outputString;

  //convert to pointxyzrgba type
  pcl::PCLPointCloud2 pcl_pc2;
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);//convert to pointer to satisfy function requirement

  pcl_conversions::toPCL(*input, pcl_pc2);
  pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 

  // stringstream to store compressed point cloud
  std::stringstream compressedData;
  // output pointcloud
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());

  // compress point cloud
  PointCloudEncoder->encodePointCloud (temp_cloud, compressedData);

  output = compressedData.str();
  outputString.data = output;

  // Publish the data.
  pub.publish (outputString);
}

int
main (int argc, char** argv)
{

  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  //\\
  //setup compression algorithm

  bool showStatistics = false;
  pcl::io::compression_Profiles_e compressionProfile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;

  // instantiate point cloud compression for encoding and decoding

  PointCloudEncoder = new ...
(more)
edit retag flag offensive close merge delete

Comments

1

Just a note: a string is not a binary string necessarily. strings terminate on \0 characters, binary strings don't.

I've not looked at what the output of PointCloudEncoder->encodePointCloud(..) is, but if it is a regular binary blob (ie: doesn't take any precautions) then it could be your data is getting truncated. That would be something to check.

gvdhoorn gravatar image gvdhoorn  ( 2020-06-10 01:17:37 -0500 )edit

Yes, you were correct. This was indeed the problem. I converted the data to uint8 to send them with ROS which solved that problem.

toml gravatar image toml  ( 2020-06-15 14:33:06 -0500 )edit