Unable to visualise point cloud in RViz even with header.frame_id selected
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:
Any help would be much appreciated.
I'm not sure it really matters, but you seem to be setting
width
andheight
, but notdepth
of your pointcloud. A cloud with a depth of0
would perhaps not work.@gvdhoorn I thought I have here:
cloud.data.resize(cloud.row_step * cloud.height);
wherecloud.row_step = cloud.point_step * cloud.width;
. Am I missing something?No. I hadn't had my coffee yet. Ignore my comment.
@gvdhoorn All good. I did hope it could be as straightforward as that though... :(
Btw: I rarely create
PointCloud2
messages by hand. Is there any particular reason you're not creating a regularpcl::PointCloud<>
and then convert it usingpcl::toROSMsg(..)
(frompcl_conversions
)?Also documented here.
@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.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.
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