Ask Your Question
0

Unable to visualise point cloud in RViz even with header.frame_id selected

asked 2019-09-03 00:44:40 -0600

mun gravatar image

updated 2019-09-03 12:46:19 -0600

Hi all, I very new to ROS, so please bear with me. I am trying to render sample point cloud in the RViz, but for some reason it just won't show. I have set fixed frame to map which is the value of header.frame_id. rostopic echo on the point cloud topic returns data, so my take is that the point cloud is correctly generated but cannot be properly displayed in rviz. Here's the code for publishing the point cloud:

void Node::PublishMapPoints (std::vector<ORB_SLAM2::MapPoint*> map_points) {
  sensor_msgs::PointCloud2 cloud = MapPointsToPointCloud (map_points);
  map_points_publisher_.publish (cloud);
}

sensor_msgs::PointCloud2 Node::MapPointsToPointCloud (std::vector<ORB_SLAM2::MapPoint*> map_points) {
  if (map_points.size() == 0) {
    std::cout << "Map point vector is empty!" << std::endl;
  }

  sensor_msgs::PointCloud2 cloud;

  const int num_channels = 3; // x y z

  cloud.header.stamp = current_frame_time_;
  cloud.header.frame_id = "map";
  cloud.height = 1;
  cloud.width = map_points.size();
  cloud.is_bigendian = false;
  cloud.is_dense = true;
  cloud.point_step = num_channels * sizeof(float);
  cloud.row_step = cloud.point_step * cloud.width;
  cloud.fields.resize(num_channels);

  std::string channel_id[] = { "x", "y", "z"};
  for (int i = 0; i<num_channels; i++) {
    cloud.fields[i].name = channel_id[i];
    cloud.fields[i].offset = i * sizeof(float);
    cloud.fields[i].count = 1;
    cloud.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
  }

  cloud.data.resize(cloud.row_step * cloud.height);

    unsigned char *cloud_data_ptr = &(cloud.data[0]);

  float data_array[num_channels];
  for (unsigned int i=0; i<cloud.width; i++) {
    if (map_points.at(i)->nObs >= min_observations_per_point_) {
      data_array[0] = map_points.at(i)->GetWorldPos().at<float> (2); //x. Do the transformation by just reading at the position of z instead of x
      data_array[1] = -1.0* map_points.at(i)->GetWorldPos().at<float> (0); //y. Do the transformation by just reading at the position of x instead of y
      data_array[2] = -1.0* map_points.at(i)->GetWorldPos().at<float> (1); //z. Do the transformation by just reading at the position of y instead of z
      //TODO dont hack the transformation but have a central conversion function for MapPointsToPointCloud and TransformFromMat

      memcpy(cloud_data_ptr+(i*cloud.point_step), data_array, num_channels*sizeof(float));
    }
  }

  return cloud;
}

Here's my RViz:

image description

Any help would be much appreciated.

edit retag flag offensive close merge delete

Comments

I'm not sure it really matters, but you seem to be setting width and height, but not depth of your pointcloud. A cloud with a depth of 0 would perhaps not work.

gvdhoorn gravatar imagegvdhoorn ( 2019-09-03 02:36:20 -0600 )edit

@gvdhoorn I thought I have here: cloud.data.resize(cloud.row_step * cloud.height); where cloud.row_step = cloud.point_step * cloud.width;. Am I missing something?

mun gravatar imagemun ( 2019-09-03 05:25:03 -0600 )edit

No. I hadn't had my coffee yet. Ignore my comment.

gvdhoorn gravatar imagegvdhoorn ( 2019-09-03 05:28:43 -0600 )edit

@gvdhoorn All good. I did hope it could be as straightforward as that though... :(

mun gravatar imagemun ( 2019-09-03 05:30:42 -0600 )edit
1

Btw: I rarely create PointCloud2 messages by hand. Is there any particular reason you're not creating a regular pcl::PointCloud<> and then convert it using pcl::toROSMsg(..) (from pcl_conversions)?

Also documented here.

gvdhoorn gravatar imagegvdhoorn ( 2019-09-03 05:31:11 -0600 )edit

@gvdhoorn Oh, I'll try that. I should also add that rostopic echo on the map points topic gives data but it's all zeros.

mun gravatar imagemun ( 2019-09-03 08:23:49 -0600 )edit

I definitely agree with @gvdhoorn, it's way easy that way.

Also, when you say it's all zeros, what do you mean? If all of your points are honestly 0,0,0, your point cloud is rather boring and all the points are going to be at the origin. Take a look at your Rviz screenshot, there is one white dot at the origin. Try increasing the point size to 0.1m or something in Rviz and if the white dot at the origin is larger, all of your points are probably sitting at the origin.

SamsAutonomy gravatar imageSamsAutonomy ( 2019-09-03 09:27:28 -0600 )edit

I've fixed my code now that they aren't all zeros. I'm getting some variations in the point cloud, but it's clearly not a point cloud. Here's a video: https://youtu.be/Yr9Qsx_Bpns

mun gravatar imagemun ( 2019-09-03 12:51:49 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-09-03 13:22:51 -0600

mun gravatar image

I indeed had the scale wrong. It's all good now! Thanks!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-09-03 00:44:40 -0600

Seen: 44 times

Last updated: Sep 03