Skip to content

Instantly share code, notes, and snippets.

@vijaravind
Last active April 22, 2022 11:19
Show Gist options
  • Save vijaravind/8a54e7f131b50af87392 to your computer and use it in GitHub Desktop.
Save vijaravind/8a54e7f131b50af87392 to your computer and use it in GitHub Desktop.
Sample code to concatenate ROS PointCloud (not PointCloud2) data
/**
* \brief Function to merge the vectors of geometry_msgs::Point32
* \param vin : input vector
* \param vout : output vector (output value)
*/
void merge_point32_vector(const std::vector<geometry_msgs::Point32>& vin, std::vector<geometry_msgs::Point32>& vout)
{
vout.reserve(vout.size() + vin.size());
vout.insert(vout.end(), vin.begin(), vin.end());
}
/**
* \brief Function to merge two PointCloud data after checking if they are in they have the same `frame_id`.
* \param cloud_in1, cloud_in2 : Two input PointClouds
* \param cloud_out : Output PointCloud
* \return true if succesful and false otherwise
*/
bool merge_point_cloud(const sensor_msgs::PointCloud& cloud_in1, const sensor_msgs::PointCloud& cloud_in2, sensor_msgs::PointCloud& cloud_out)
{
// check if both have the same frame_id
if(cloud_in1.header.frame_id != cloud_in2.header.frame_id)
return false;
// set cloud_out frame_id
cloud_out.header.frame_id = cloud_in1.header.frame_id;
// merge the clouds one by one
merge_point32_vector(cloud_in1.points, cloud_out.points);
merge_point32_vector(cloud_in2.points, cloud_out.points);
return true;
}
@aaryanmurgunde
Copy link

This is really good, works like a charm.
Thanks !

@Petros626
Copy link

It's possible to do that in Python? I have installed the pcl library, but dont get the required imports and functions

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment