c++ - RVIZ : Display own point cloud

Question

我尝试使用高斯分布构建自己的点云。 rviz 的可视化不起作用。

这是我创建点云的方法

int sizeOfCloud = 1000;
keypoints.points.resize(sizeOfCloud);
getRandomPointCloud(keypoints, 100, 100, sizeOfCloud);

keypoints.header.frame_id = "base_link";
keypoints.header.stamp = ros::Time::now();
keypoints_publisher.publish(keypoints);

这里是 getRandomPointCloud 函数:

void getRandomPointCloud(sensor_msgs::PointCloud& pc, int centerX, int centerY, int& sizeOfCloud) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> distX(centerX, 10);
    std::normal_distribution<> distY(centerY, 10);


    for (int i = 0; i < pc.points.size(); i++) {
        double xValue = distX(gen);
        double yValue = distY(gen);
        std::cout << std::round(xValue) << std::endl;
        pc.points[i].x = std::round(xValue);
        pc.points[i].y = std::round(yValue);
    }
    std::cout << "done" << std::endl;
}

正如我所说，它不能在 rviz 中显示。我确实按主题选择，选择适当的主题，然后屏幕上什么也没有。主题是正确的，如果我将网格设置为 base_link，那么与主题相关的所有内容都可以。也许我必须在 rviz 中设置一个特殊属性，或者我没有正确构建我的点云。

编辑:

这是rviz的截图

现在我认为问题更多是关于无法解决的“base_link”tf 主题。如果我尝试映射我的 tf 树，则没有条目。如何在我的 tf 树中设置 base_link。或者我的目的还有另一种可能性吗？

Answer 1

留言sensor_msgs::PointCloud pc 有一个 Point32 数组，它又具有 x、y 和 z 值。您正在设置每个点的 x 和 y 值但您缺少 z 值。

我不确定 rviz 可视化工具是否也需要 channel 信息。如果尽管有 z 值点云仍然不可见，则设置 channel 信息。 channel 是 sensor_msgs::PointCloud 中的一个数组称为 channels，类型为 ChannelFloat32 .如果您有深度信息，您可以使用单个 channel :

sensor_msgs::ChannelFloat32 depth_channel;
depth_channel.name = "distance";
for (int i = 0; i < pc.points.size(); i++) {
  depth_channel.values.push_back(0.43242); // or set to a random value if you like
}
// add channel to point cloud
pc.channels.push_back(depth_channel);

为了在 rviz 中看到它，多次发布消息也很重要，而且在处理 TF 时，您经常需要更新标题中的时间戳。

顺便说一句，您正在将点散布在 100 米/10 米的点周围!

这是我的例子。

这是适合我的代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <string>
#include <random>

void getRandomPointCloud(sensor_msgs::PointCloud& pc,
                         double centerX,
                         double centerY,
                         int& sizeOfCloud) {
  std::random_device rd;
  std::mt19937 gen(rd());
  std::normal_distribution<> distX(centerX, 2.);
  std::normal_distribution<> distY(centerY, 2.);

  for (int i = 0; i < pc.points.size(); i++) {
    double xValue = distX(gen);
    double yValue = distY(gen);
    pc.points[i].x = xValue;
    pc.points[i].y = yValue;
    pc.points[i].z =
        std::exp(-((xValue * xValue) + (yValue * yValue)) / 4.);
  }
  sensor_msgs::ChannelFloat32 depth_channel;
  depth_channel.name = "distance";
  for (int i = 0; i < pc.points.size(); i++) {
    depth_channel.values.push_back(pc.points[i].z); // or set to a random value if you like
  }
  // add channel to point cloud
  pc.channels.push_back(depth_channel);
}

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

  int sizeOfCloud = 100000;
  sensor_msgs::PointCloud keypoints;
  keypoints.points.resize(sizeOfCloud);
  getRandomPointCloud(keypoints, 0.5, 0.5, sizeOfCloud);

  keypoints.header.frame_id = "base_link";
  keypoints.header.stamp = ros::Time::now();

  auto keypoints_publisher =
      nh.advertise<sensor_msgs::PointCloud>("point_cloud", 10);
  ros::Rate rate(30);
  while (ros::ok()) {
    keypoints.header.stamp = ros::Time::now();
    keypoints_publisher.publish(keypoints);
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}