Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
chuyaqifei opened this issue Jul 1, 2023 · 6 comments

Comments

@chuyaqifei
Copy link

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
Copy link

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
Copy link
Author

chuyaqifei commented Jul 20, 2023

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
Copy link

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
Screenshot from 2023-07-20 22-07-34

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
Copy link
Author

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?
one_arrow

@YipengWang1040
Copy link

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

@YipengWang1040
Copy link

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants