tf::MessageFilter does not work with pcl::PointCloud messages

[bricerebsamen]

The header is not compatible.

I can see 2 workarounds:

• use some template magic to convert the pcl header to ros header when appropriate
• allow to provide a custom function to get the time and frame of the message (this way it could work with unstamped messages as well)

[wjwwood]

In case you were not aware, this is related: https://github.com/ros-perception/pcl_conversions

[bricerebsamen]

actually when I was saying "use some template magic" I was referring to those toPCL and fromPCL functions. but pcl_conversions does not provide a solution.

In my case, I'm working with pcl::PointCloud. I have a filter that derives from message_filters::SimpleFilter< pcl::PointCloudpcl::PointXYZ >. I want to connect it to a tf filter, and it does not work because of header incompatibilities.

After taking a closer look at pcl_conversions, I can see that by using moveFromPCL and moveToPCL it might be possible to redesign my code so that it works more with ros::PointCloud2 messages.

However, I think it would be a nice addition to tf::MessageFilter to support PCL headers or more generic data with a custom header extractor hook. If you don't think so, please go ahead and just close the issue...

On 02/06/2014 02:32 PM, William Woodall wrote:

In case you were not aware, this is related: https://github.com/ros-perception/pcl_conversions

— Reply to this email directly or view it on GitHub https://github.com/ros/geometry/issues/55#issuecomment-34382427.

[wjwwood]

No I think that would be fine to have direct support, though I'm not sure how it would look exactly, but I was just linking to pcl_conversions as a reference not to imply it had a solution already.

[bricerebsamen]

I was thinking about something along the line of the toMsg and fromMsg functions in the geodesy package: https://github.com/ros-geographic-info/geographic_info/blob/master/geodesy/include/geodesy/wgs84.h

I will try to come up with an implementation, at least for the PCL header case.

[bricerebsamen]

Adding the following lines in pcl_conversions.h could be a solution

    template<>
    struct FrameId<pcl::PCLPointCloud2>
    {
      static std::string* pointer(pcl::PCLPointCloud2& m) { return &m.header.frame_id; }
      static std::string const* pointer(const pcl::PCLPointCloud2& m) { return &m.header.frame_id; }
      static std::string value(const pcl::PCLPointCloud2& m) { return m.header.frame_id; }
    };

    template<>
    struct TimeStamp<pcl::PCLPointCloud2>
    {
      static ros::Time* pointer(typename boost::remove_const<pcl::PCLPointCloud2>::type &m) { return 0; }
      static ros::Time const* pointer(const pcl::PCLPointCloud2& m) { return 0; }
      static ros::Time value(const pcl::PCLPointCloud2& m) { return pcl_conversions::fromPCL(m.header).stamp; }
    };

However, TimeStamp pointer functions would return NULL. Currently (i.e. without those lines), the default template returns NULL for the stamp and frame pointers. So it cannot make it worst I think. But it's a bit in-coherent: value() returns a sensible value, but not the pointer functions, even though one might expect that they would return the same thing.

Any thought?

[bricerebsamen]

turns out this does not help with pcl::PointCloud

in pcl_ros/point_cloud.h someone (@wjwwood ?) specialized ros::message_traits::TimeStamp for the pcl::PointCloud<pcl::PointXYZ> type, with the following comment:

// This is bad because it will only work for pcl::PointXYZ, but I can't
// find another way around ambiguous partial template specialization...

This is because if we try to do a partial specialization like this:

template<typename T>
struct TimeStamp<pcl::PointCloud<T> >

Then it's ambiguous with the default template declaration in message_traits.h:

template<typename M>
struct TimeStamp<M, typename boost::enable_if<HasHeader<M> >::type >
{
  static ros::Time* pointer(typename boost::remove_const<M>::type &m) { return &m.header.stamp; }
  static ros::Time const* pointer(const M& m) { return &m.header.stamp; }
  static ros::Time value(const M& m) { return m.header.stamp; }
};

In which M=pcl::PointCloud<T> and HasHeader<M> evaluates to true. I tried to specialize like this:

template<typename T>
struct TimeStamp<pcl::PointCloud<T>, TrueType>

without success.

I think the underlying problem here is that HasHeader<pcl::PointCloud<T>> should return false. FrameId and TimeStamp types should then be specialized for pcl::PointCloud<T>. But I'm not quite sure of the implications of that, in particular for serialization.

[bricerebsamen]

I seem to have found a solution:

• HasHeader returns false for pcl::PointCloud<T>
• FrameId and TimeStamp specialized for pcl::PointCloud<T>
• message_filter modified to use FrameId and TimeStamp everywhere instead of accessing the message header directly

These require to modify both the geometry repo and the perception_pcl repo. I will create respective PRs.

However, I wonder if there will be hidden consequences to modifying HasHeader<pcl::PointCloud<T>>?