CODEWITHSUNDEEP
c++ros
BeJay
BeJay

Reputation: 425

RVIZ: Display own point cloud

I try to build my own point cloud with a gaussian distribution. The visualization with rviz doesn't work.

Here is how I create the 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);

and here is the function 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;
}

As I said, it can't be displayed in rviz. I do select by topic, select the proper topic and then there is nothing on the screen. Topic is correct and if I set the grid to base_link then everything with the topic is okay. Maybe I have to set a special attribute in rviz or I don't build my pointcloud correctly.

Edit:

Here is a screenshot from rviz Screenshot from rviz

Now I think the problem is more about the "base_link" tf topic which can't get resolved. If I try to map my tf tree then there is no entry. How do I set the base_link in my tf tree. Or is there another possibility for my purpose?

Upvotes: 3

Views: 6138

Answers (2)

James
James

Reputation: 184

You might try zooming out a bit...

enter image description here

and of course ensure the Fixed Frame matches the frame in your message. You can see I also made the points larger (1.0 meter) and used a flat colour to ensure visibility over your enormous scale

enter image description here

Upvotes: 0

cassinaj
cassinaj

Reputation: 1043

The message sensor_msgs::PointCloud pc has an array of Point32 which in turn has x, y and z values. You are setting the x and y values of each point but you are missing the z value.

I'm not sure if the rviz visualizer also requires channel information. If the point cloud is still not visible despite the z value, then set the channel information. The channel is an array in sensor_msgs::PointCloud called channels which is of type ChannelFloat32. If you have depth information you can use a single 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);

It is also important to publish the message more than once in order to see it in rviz and often when dealing with TF you need to update the time stamp in the header.

Btw you are spreading the points around the point 100meter/10meter thats way out!

Here is my example. 2D Gaussian generated by the point cloud

Here is the code that works for me

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

Upvotes: 4

Related Questions