sending PCL binary stream
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 ...
Just a note: a string is not a binary string necessarily.
string
s 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.Yes, you were correct. This was indeed the problem. I converted the data to uint8 to send them with ROS which solved that problem.
Hi toml, I have encountered the same problem as you, but I can't solve it after transferring the stringstream to std_msgs::UInt8MultiArray, can you tell me your solution? Thank you so much!