How to merge two sensor_msgs/PointCloud2.msg from two lidar
I want to merge 2 lidar data. My code is below
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include <sensor_msgs/point_cloud2_iterator.h>
#include <iostream>
#include <vector>
#include <algorithm>
ros::Publisher *pub;
sensor_msgs::PointCloud2 temp;
void callback2(const sensor_msgs::PointCloud2ConstPtr &cloud_msg2)
{
temp.header = cloud_msg2->header;
temp.height = cloud_msg2->height;
temp.fields = cloud_msg2->fields;
temp.is_bigendian = cloud_msg2->is_bigendian;
temp.point_step = cloud_msg2->point_step;
temp.is_dense = cloud_msg2->is_dense;
temp.width = cloud_msg2->width;
temp.row_step = cloud_msg2->row_step;
temp.data.assign(cloud_msg2->data.begin(), cloud_msg2->data.end());
for (sensor_msgs::PointCloud2Iterator<float>
iter_x(temp, "x"), iter_y(temp, "y"), iter_z(temp, "z");
iter_x != iter_x.end();
++iter_x, ++iter_y, ++iter_z)
{//transform
float x = *iter_x;
float y = *iter_y;
float z = *iter_z;
*iter_x = -0.0315695*x + 0.492979*y + -0.869468*z + -0.326036;
*iter_y = -0.557120*x + -0.730911*y + -0.394190*z + -0.155932;
*iter_z = -0.829832*x + 0.471954*y + 0.297723*z + -0.385833;
}
}
void callback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
sensor_msgs::PointCloud2 output;
output.header = cloud_msg->header;
output.height = cloud_msg->height;
output.fields = cloud_msg->fields;
output.is_bigendian = cloud_msg->is_bigendian;
output.point_step = cloud_msg->point_step;
output.is_dense = cloud_msg->is_dense;
output.width = cloud_msg->width + temp.width;
output.row_step = cloud_msg->row_step + temp.row_step;
//merge
merge(cloud_msg->data.begin(),cloud_msg->data.end(),temp.data.begin(),temp.data.end(),output.data.begin());
std::cout<<"merge complete"<<std::endl;
pub->publish(output);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "merge_cloudpoint");
ros::NodeHandle n;
static ros::Publisher pub_ = n.advertise<sensor_msgs::PointCloud2>("velodyne_points",10);
pub = &pub_;
ROS_INFO("start");
ros::Subscriber sub = n.subscribe("points1", 100, callback);
ros::Subscriber sub2 = n.subscribe("points2", 100, callback2);
ros::spin();
return 0;
}
It can compile, but arise error when running. The error information is below. I need your help.
Segmentation fault (core dumped)
Hi @Scarley,
The problem may be in dealing with vectors. First thing is why you are using a STL method (merge). If you are going to work with PointClouds why not using the PCL library that already have a operator to merge pointclouds (+=).
Second is that you will need the proper namespace for the method:
std::merge