Ask Your Question
0

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

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

mun gravatar image

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

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 image gvdhoorn  ( 2019-09-03 02:36:20 -0500 )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 image mun  ( 2019-09-03 05:25:03 -0500 )edit

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

gvdhoorn gravatar image gvdhoorn  ( 2019-09-03 05:28:43 -0500 )edit

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

mun gravatar image mun  ( 2019-09-03 05:30:42 -0500 )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 image gvdhoorn  ( 2019-09-03 05:31:11 -0500 )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 image mun  ( 2019-09-03 08:23:49 -0500 )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 image SamsAutonomy  ( 2019-09-03 09:27:28 -0500 )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 image mun  ( 2019-09-03 12:51:49 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

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

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 -0500

Seen: 212 times

Last updated: Sep 03 '19