RVIZ: показать собственное облако точек

Question

Я пытаюсь создать собственное облако точек с гауссовским распределением. Визуализация с помощью rviz не работает.

Вот как я создаю PointCloud

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 или я не правильно построю свой pointcloud.

Edit:

Вот скриншот из rviz

Теперь я думаю, что проблема больше о «base_link» ТФ тему, которая не может получить решен. Если я попытаюсь отобразить мое дерево tf, то нет записи. Как установить base_link в моем tf-дереве. Или есть еще одна возможность для моей цели?

Answer 1

Сообщение sensor_msgs::PointCloud pc имеет массив Point32, который, в свою очередь, имеет значения x, y и z. Вы устанавливаете значения x и y для каждой точки , но вам не хватает значения z.

Я не уверен, что визуализатор rviz также требует информации о канале. Если облако точек все еще не видно, несмотря на значение z, задайте информацию о канале. Канал представляет собой массив в sensor_msgs::PointCloud под названием channels, который имеет тип ChannelFloat32. Если у вас есть информация по глубине вы можете использовать один канал:

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 вам необходимо обновить штамп времени в заголовке.

Btw вы распространяете точки вокруг точки 100meter/10meter thats out out!

Вот мой пример.

Вот код, который работает для меня

#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; 
}