How to build the sensor_msgs::PointCloud2 message required by the plugin?

[chuyaqifei]

I've tried to use pcl_conversion to convert pcl::pointcloudpcl::xyznormal into sensor_msgs::PointCloud2, but the rviz failed with segmentation fault. Could you give a example to show how to build the sensor_msgs::PointCloud2 message required by the plugin? Or how to convert a pcl pointcloud in to the required sensor_msgs::PointCloud2 message?

[YipengWang1040]

As long as the message contains position fields 'x', 'y', 'z' and normal fields 'normal_x', 'normal_y', and 'normal_z', it should work, despite how the message was generated. pcl_conversion should also work.

Can you provide several frames of your message in a rosbag that causes the crash? I'll look into it.

[chuyaqifei]

In my code, a .pcd file is loaded and then used for estimating normals. The normals are generated with "pcl::NormalEstimation", and can be visualized properly with pcl viewer. However, the "pointcloud2_normal_rviz_plugin" cannot work. My code is pasted here:

#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/console.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

ros::Publisher sample_points_pub;
std::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

int main(int argc, char **argv) {
  ros::init(argc, argv, "sample_points_publisher");
  ros::NodeHandle nh;

  sample_points_pub =
      nh.advertise<sensor_msgs::PointCloud2>("sample_points", 10000);

  pcl::PointCloud<pcl::PointXYZ> cloud, cloud_filtered;
  pcl::io::loadPCDFile("debris_cut.pcd", cloud);
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud.makeShared());
  sor.setLeafSize(0.2f, 0.2f, 0.2f);
  sor.filter(cloud_filtered);

  pcl::PointCloud<pcl::Normal> normals;
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
      new pcl::search::KdTree<pcl::PointXYZ>());
  tree->setInputCloud(cloud_filtered.makeShared());
  ne.setInputCloud(cloud_filtered.makeShared());
  ne.setSearchSurface(cloud_filtered.makeShared());
  ne.setRadiusSearch(0.5);
  ne.setSearchMethod(tree);
  ne.compute(normals);
  std::cout << "normals size = " << normals.size() << std::endl;

  pcl::PointCloud<pcl::PointNormal> normal_pcd;
  pcl::concatenateFields(cloud_filtered, normals, normal_pcd);

  viewer = std::shared_ptr<pcl::visualization::PCLVisualizer>(
      new pcl::visualization::PCLVisualizer("Viewer"));
  viewer->setBackgroundColor(0, 0, 0);
  viewer->addPointCloud<pcl::PointNormal>(normal_pcd.makeShared(), "foo");
  viewer->addPointCloudNormals<pcl::PointNormal, pcl::PointNormal>(
      normal_pcd.makeShared(), normal_pcd.makeShared(), 25, 0.15,
      "normals");
  ros::Rate rate(1);
  while (ros::ok()) {
    viewer->spinOnce();
    if (normals.size() > 0) {
      sensor_msgs::PointCloud2 pcd_msg;
      pcl::toROSMsg(normal_pcd, pcd_msg);
      pcd_msg.header.frame_id = "map";
      pcd_msg.header.stamp = ros::Time::now();
      sample_points_pub.publish(pcd_msg);
    }
    rate.sleep();
  }
}

[YipengWang1040]

I tried your code using one PCD file in the tutorial capture0001.pcd and it works fine. The only modification I made is to change line 26 to

  pcl::io::loadPCDFile(argv[1], cloud);

The result looks like

Have you tried other PCD files? Maybe you can upload debris_cut.pcd somewhere so that I can have a look into that file.

[chuyaqifei]

When i use the tutorial capture0001.pcd, it still not works. Note that i run the code on ubuntu18.04 and ros-melodic. When compling the rviz plugin, it failed with errors because <rviz/ogre_helpers/ogre_vector.h and version_check.h> were not found. To solve it, I commented the headers and passed the compilation. However, only an arrow can be shown in rviz. Is there somthing wrong with my environments?

[YipengWang1040]

The plugin was developed and tested on Ubuntu 20. I'll take a look.

[YipengWang1040]

There is some difference in RViz and PCL between melodic and noetic. Removing those two headers should be a valid workaround, but I do not have a physical machine running Ubuntu 18, and in my VM the plugin consistently crashes with "Cannot create GL vertex buffer" message, which I found could be a OpenGL driver issue. In my tests disabling 3D acceleration in VM settings makes the situation worse.

Since the performance in my VM is extremely inconsistent, I cannot further examine and fix this. You may update your environments and try if the plugin works in your situation under Ubuntu 20.