search

ros-perception / perception_pcl

PCL (Point Cloud Library) ROS interface stack
http://wiki.ros.org/perception_pcl
422 stars 332 forks source link

Timestamp precision loss when subscribing to PointCloud2 as PointCloud #115

Open thebyohazard opened 8 years ago

thebyohazard commented 8 years ago

PCLNodelet has a message_filters::Subscriber<PointCloud> called sub_input_filter_. I don't know how, but if the actual topic is a PointCloud2, the nodelet can still subscribe to it and gets the message as a PointCloud. This works fine for most things, but apparently the timstamp of the header for PointCloud2 is more precise than the header for a PointCloud. For example, the input cloud timestamp (as viewed by rostopic echo) will actually be 

stamp:
   secs: 1455912720
   nsecs: 787587873

but the nodelet in its callback will see

stamp: 1455912720787587

Which is equivalent to 

stamp:
   secs: 1455912720
   nsecs: 787587000

The header of output topics (such as the inliers topic) published by the nodelet inherit this lower precision stamp. This can cause issues for other nodelets in a pipeline such as an ExtractIndices nodelet using an exact timestamp policy in a message synchronizer to match a cloud and inliers.

My original question is here.

I needed a segmenter to work, so I fixed it by creating a second input_indices_callback that subscribes to a PointCloud2ConstPtr instead of a PointCloudConstPtr. I do have to create a PointCloud from the PointCloud2 for the segmenter to work, but at least I can propagate a precise timestamp that I didn't have before. My fix is a hack and I believe the right way to do it would be to change the whole of PCLNodelet to use PointCloud2s for subscription and only create PointClouds when needed for pcl functions that need them. (Or perhaps pcl functions that use PointCloud2 will be created before then) Anyway, I don't have the time to do that now, but I wanted to make the bug known to the community in case somebody else wanted to do something.

wjwwood commented 8 years ago

The pcl functions take pcl versions of those structures, i.e. there is a difference between pcl::PCLPointCloud and ROS's sensor_msgs::PointCloud. One of the differences, as you noticed, is that they use a different time unit, see: https://github.com/ros-perception/pcl_conversions/blob/indigo-devel/include/pcl_conversions/pcl_conversions.h#L79-L89

I think the only solution is to propagate the high precision time stamp across the ros -> pcl -> ros boundary, as you've suggested. Unfortunately, I don't have time to look into this with you. Maybe one of the maintainers will be.

k-okada commented 8 years ago

Hi, only solution we found is to use sensor_msg/PointCloud2 at https://github.com/ros-perception/perception_pcl/blob/kinetic-devel/pcl_ros/include/pcl_ros/features/feature.h#L130

      virtual void computePublish (const PointCloudInConstPtr &cloud, 
                                   const PointCloudInConstPtr &surface,
                                   const IndicesPtr &indices) = 0;

as we had in https://github.com/ros-perception/perception_pcl/blob/kinetic-devel/pcl_ros/include/pcl_ros/filters/filter.h#L126

      void 
      computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices);

and copy nano second timestamp at https://github.com/ros-perception/perception_pcl/blob/kinetic-devel/pcl_ros/src/pcl_ros/filters/filter.cpp#L98

  // Copy timestamp to keep it
  cloud_tf->header.stamp = input->header.stamp;

  // Publish a boost shared ptr
  pub_output_.publish (cloud_tf);

If some one have another idea, please let me know

Cc: @wkentaro