ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to merge two sensor_msgs/PointCloud2.msg from two lidar

asked 2020-03-19 19:47:40 -0500

Scarley gravatar image

updated 2020-03-20 00:49:46 -0500

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)

edit retag flag offensive close merge delete

Comments

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

Weasfas gravatar image Weasfas  ( 2020-03-24 15:17:17 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-05-07 04:11:40 -0500

Hi @Scarley, Did you be able to merge two laserscans. If you did could you share with me the solution, I have kind of problem with merging two laserscans.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-03-19 19:47:40 -0500

Seen: 415 times

Last updated: Mar 20 '20