perception_pcl理解 --- pcl_conversions 与 pcl_ros

本篇文章记录了下 perception_pcl 包中的一些常用函数，已做以后查找。

希望本文可以让您直接搞清楚ros中的pcl相关的知识。

1 pcl_conversions

pcl_conversions 包中只有一个有效文件，就是i include/pcl_conversions/pcl_conversions.h ，这个文件里定义了一些常用的 进行数据类型转换的函数。

其wiki上的链接为：http://wiki.ros.org/pcl_conversions?distro=noetic

看代码，发现，这个文件里有3个命名空间，分别是 pcl_conversions, pcl, 与ros。其中常用的数据类型转换的函数在前两个命名空间中。

1.1 PCL的时间戳 与 ros的时间戳 间的转换

#ifndef PCL_CONVERSIONS_H__
#define PCL_CONVERSIONS_H__

namespace pcl_conversions {

  /** PCLHeader <=> Header **/
  // PCL的时间戳 与 ros的时间戳 间的转换

  inline
  void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
  {
    stamp.fromNSec(pcl_stamp * 1000ull);  // Convert from us to ns
  }

  inline
  void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
  {
    pcl_stamp = stamp.toNSec() / 1000ull;  // Convert from ns to us
  }

  inline
  ros::Time fromPCL(const std::uint64_t &pcl_stamp)
  {
    ros::Time stamp;
    fromPCL(pcl_stamp, stamp);
    return stamp;
  }

  inline
  std::uint64_t toPCL(const ros::Time &stamp)
  {
    std::uint64_t pcl_stamp;
    toPCL(stamp, pcl_stamp);
    return pcl_stamp;
  }
} // namespace pcl_conversions

1.2 PCL的消息头 与 ros的消息头 间的转换

namespace pcl_conversions {
  /** PCLHeader <=> Header **/
  // PCL的消息头 与 ros的消息头 间的转换
  inline
  void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::Header &header)
  {
    fromPCL(pcl_header.stamp, header.stamp);
    header.seq = pcl_header.seq;
    header.frame_id = pcl_header.frame_id;
  }

  inline
  void toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header)
  {
    toPCL(header.stamp, pcl_header.stamp);
    pcl_header.seq = header.seq;
    pcl_header.frame_id = header.frame_id;
  }

  inline
  std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
  {
    std_msgs::Header header;
    fromPCL(pcl_header, header);
    return header;
  }

  inline
  pcl::PCLHeader toPCL(const std_msgs::Header &header)
  {
    pcl::PCLHeader pcl_header;
    toPCL(header, pcl_header);
    return pcl_header;
  }
} // namespace pcl_conversions

1.3 pcl::PCLPointField 与 sensor_msgs::PointField 间的转换

namespace pcl_conversions {
  /** PCLPointField <=> PointField **/
  // pcl::PCLPointField 与 sensor_msgs::PointField 间的转换
  inline
  void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf)
  {
    pf.name = pcl_pf.name;
    pf.offset = pcl_pf.offset;
    pf.datatype = pcl_pf.datatype;
    pf.count = pcl_pf.count;
  }

  inline
  void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::PointField> &pfs)
  {
    pfs.resize(pcl_pfs.size());
    std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
    int i = 0;
    for(; it != pcl_pfs.end(); ++it, ++i) {
      fromPCL(*(it), pfs[i]);
    }
  }

  inline
  void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf)
  {
    pcl_pf.name = pf.name;
    pcl_pf.offset = pf.offset;
    pcl_pf.datatype = pf.datatype;
    pcl_pf.count = pf.count;
  }

  inline
  void toPCL(const std::vector<sensor_msgs::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs)
  {
    pcl_pfs.resize(pfs.size());
    std::vector<sensor_msgs::PointField>::const_iterator it = pfs.begin();
    int i = 0;
    for(; it != pfs.end(); ++it, ++i) {
      toPCL(*(it), pcl_pfs[i]);
    }
  }
} // namespace pcl_conversions

1.4 pcl::PCLPointCloud2 与 sensor_msgs::PointCloud2 间的转换

namespace pcl_conversions {
  /** PCLPointCloud2 <=> PointCloud2 **/
  // pcl::PCLPointCloud2 与 sensor_msgs::PointCloud2 间的转换
  inline
  void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
  {
    fromPCL(pcl_pc2.header, pc2.header);
    pc2.height = pcl_pc2.height;
    pc2.width = pcl_pc2.width;
    fromPCL(pcl_pc2.fields, pc2.fields);
    pc2.is_bigendian = pcl_pc2.is_bigendian;
    pc2.point_step = pcl_pc2.point_step;
    pc2.row_step = pcl_pc2.row_step;
    pc2.is_dense = pcl_pc2.is_dense;
  }

  inline
  void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
  {
    copyPCLPointCloud2MetaData(pcl_pc2, pc2);
    pc2.data = pcl_pc2.data;
  }

  inline
  void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
  {
    copyPCLPointCloud2MetaData(pcl_pc2, pc2);
    pc2.data.swap(pcl_pc2.data);
  }

  inline
  void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
  {
    toPCL(pc2.header, pcl_pc2.header);
    pcl_pc2.height = pc2.height;
    pcl_pc2.width = pc2.width;
    toPCL(pc2.fields, pcl_pc2.fields);
    pcl_pc2.is_bigendian = pc2.is_bigendian;
    pcl_pc2.point_step = pc2.point_step;
    pcl_pc2.row_step = pc2.row_step;
    pcl_pc2.is_dense = pc2.is_dense;
  }

  inline
  void toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
  {
    copyPointCloud2MetaData(pc2, pcl_pc2);
    pcl_pc2.data = pc2.data;
  }

  inline
  void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
  {
    copyPointCloud2MetaData(pc2, pcl_pc2);
    pcl_pc2.data.swap(pc2.data);
  }


} // namespace pcl_conversions

1.5 pcl::PointCloud 与 sensor_msgs::PointCloud2 间的转换

namespace pcl {


  /** Provide to/fromROSMsg for sensor_msgs::PointCloud2 <=> pcl::PointCloud<T> **/
  // pcl::PointCloud 与 sensor_msgs::PointCloud2 间的转换
  
  template<typename T>
  void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
    pcl_conversions::moveFromPCL(pcl_pc2, cloud);
  }

  template<typename T>
  void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
  }

  template<typename T>
  void moveFromROSMsg(sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::moveToPCL(cloud, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
  }


} // namespace pcl

#endif /* PCL_CONVERSIONS_H__ */

2 pcl_ros

pcl_ros的文件比较多，大致可以分为3个部分。

第一个部分为4个功能文件:

• pcl_nodelet.h
• point_cloud.h
• publisher.h
• transforms.h

第二部分为使用ros实现了一些pcl的功能接口，如voxel_filter等，并通过nodelet进行调用。

第三部分为独立的一些附加功能。

2.1 功能文件

2.1.1 point_cloud.h

这个文件的内容没太看懂，首先include了很多pcl的文件，然后定义了一些指针。。。

2.1.2 publisher.h

http://wiki.ros.org/pcl_ros?distro=noetic

这部分还挺有意思的，代码不多，先放代码吧

#ifndef PCL_ROS_PUBLISHER_H_
#define PCL_ROS_PUBLISHER_H_

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/conversions.h>

#include <pcl_conversions/pcl_conversions.h>

namespace pcl_ros 
{
  class BasePublisher
  {
    public:
      void 
        advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
      {
        pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
      }

      std::string getTopic ()
      {
        return (pub_.getTopic ());
      }

      uint32_t 
        getNumSubscribers () const
      {
        // pub_.getNumSubscribers () 可以获得订阅者的数量
        return (pub_.getNumSubscribers ());
      }

      void 
        shutdown ()
      {
        // pub_.shutdown () 可以关闭发布器？？？
        pub_.shutdown ();
      }

      operator void*() const
      {
        return (pub_) ? (void*)1 : (void*)0; 
      }

    protected:
      ros::Publisher pub_;
  };

  // 可以将 pcl::PointCloud<PointT> 使用ros的发布器 发布出去，发出的数据类型为sensor_msgs::PointCloud2
  template <typename PointT>
  class Publisher : public BasePublisher
  {
    public:
      Publisher () {}

      Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
      {
        advertise (nh, topic, queue_size);
      }

      ~Publisher () {}

      inline void 
        publish (const boost::shared_ptr<const pcl::PointCloud<PointT> > &point_cloud) const
      {
        publish (*point_cloud);
      }

      inline void 
        publish (const pcl::PointCloud<PointT>& point_cloud) const
      {
        // Fill point cloud binary data
        sensor_msgs::PointCloud2 msg;
        pcl::toROSMsg (point_cloud, msg);
        pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (msg));
      }
  };

  // 发布 sensor_msgs::PointCloud2 数据类型的topic
  template <>
  class Publisher<sensor_msgs::PointCloud2> : public BasePublisher
  {
    public:
      Publisher () {}

      Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
      {
        advertise (nh, topic, queue_size);
      }

      ~Publisher () {}

      void 
        publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const
      {
        pub_.publish (point_cloud);
        //pub_.publish (*point_cloud);
      }

      void 
        publish (const sensor_msgs::PointCloud2& point_cloud) const
      {
        pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (point_cloud));
        //pub_.publish (point_cloud);
      }
  };
}
#endif  //#ifndef PCL_ROS_PUBLISHER_H_

首先，定义了一个基类 BasePublisher，之后定义了2个子类，都继承了这个类。

template class Publisher : public BasePublisher

第一个子类可以直接发布 pcl::PointCloud 数据类型的数据，因为它里边自己做了转换，转成了 sensor_msgs::PointCloud2 的数据类型。

所以，发布 pcl::PointCloud 数据类型的数据，在ros中订阅的时候会变成 sensor_msgs::PointCloud2 的数据类型，这个用起来挺方便。

第二个子类就是发布 sensor_msgs::PointCloud2 的数据，这个应该就是咱经常用的sensor_msgs::PointCloud2 发布器的所在的位置了。

2.1.3 transforms.h

这部分定义了非常多的数据坐标变换函数

将类型为 pcl::PointCloud < PointT > 的输入点云 cloud_in，根据输入的tf或者是geometry_msgs::Transform 转换为 类型为 pcl::PointCloud < PointT > 的输出点云cloud_out。

2.1.4 pcl_nodelet.h

没太看明白，好像就是定义了如何使用 nodelet 。。。

2.2 pcl功能接口

这部分在wiki上有教程
http://wiki.ros.org/pcl_ros/Tutorials

实现了5个大功能，分别为:

• features: 特征提取相关功能
• filters: 体素滤波，外点剔除等滤波器的相关功能
• io: 读写bag与pcd文件的一些相关功能
• segmentation: 语义信息提取的相关功能
• surface: 进行平面识别类似的相关功能

2.3 附加功能

复制了wiki上的内容
http://wiki.ros.org/pcl_ros?distro=noetic

2.3.1 bag_to_pcd

Reads a bag file, saving all ROS point cloud messages on a specified topic as PCD files.

Usage

 $ rosrun pcl_ros bag_to_pcd <input_file.bag> <topic> <output_directory>

Where:

<input_file.bag> is the bag file name to read.

<topic> is the topic in the bag file containing messages to save.

<output_directory> is the directory on disk in which to create PCD files from the point cloud messages. 

Example

Read messages from the /laser_tilt_cloud topic in data.bag, saving a PCD file for each message into the ./pointclouds subdirectory.

$ rosrun pcl_ros bag_to_pcd data.bag /laser_tilt_cloud ./pointclouds

2.3.2 convert_pcd_to_image

Loads a PCD file, publishing it as a ROS image message five times a second.
Published Topics
output (sensor_msgs/Image)

A stream of images generated from the PCD file. 

Usage

 $ rosrun pcl_ros convert_pcd_to_image <cloud.pcd>

Read the point cloud in <cloud.pcd> and publish it in ROS image messages at 5Hz.

2.3.3 convert_pointcloud_to_image

Subscribes to a ROS point cloud topic and republishes image messages.
Subscribed Topics
input (sensor_msgs/PointCloud2)

A stream of point clouds to convert to images. 

Published Topics
output (sensor_msgs/Image)

Corresponding stream of images. 

Examples

Subscribe to the /my_cloud topic and republish each message on the /my_image topic.

 $ rosrun pcl_ros convert_pointcloud_to_image input:=/my_cloud output:=/my_image

To view the images created by the previous command, use image_view.

 $ rosrun image_view image_view image:=/my_image

2.3.4 pcd_to_pointcloud

Loads a PCD file, publishing it one or more times as a ROS point cloud message.
Published Topics
cloud_pcd (sensor_msgs/PointCloud2)

A stream of point clouds generated from the PCD file. 

Parameters
~frame_id (str, default: /base_link)

Transform frame ID for published data. 

Usage

 $ rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ]

Where:

<file.pcd> is the (required) file name to read.

<interval> is the (optional) number of seconds to sleep between messages. If <interval> is zero or not specified the message is published once. 

Examples

Publish the contents of point_cloud_file.pcd once in the /base_link frame of reference.

 $ rosrun pcl_ros pcd_to_pointcloud point_cloud_file.pcd

Publish the contents of cloud_file.pcd approximately ten times a second in the /odom frame of reference.

 $ rosrun pcl_ros pcd_to_pointcloud cloud_file.pcd 0.1 _frame_id:=/odom

2.3.5 pointcloud_to_pcd

Subscribes to a ROS topic and saves point cloud messages to PCD files. Each message is saved to a separate file, names are composed of an optional prefix parameter, the ROS time of the message, and the .pcd extension.
Subscribed Topics
input (sensor_msgs/PointCloud2)

A stream of point clouds to save as PCD files. 

Parameters
~prefix (str)

Prefix for PCD file names created. 

~fixed_frame (str)

If set, the transform from the fixed frame to the frame of the point cloud is written to the VIEWPOINT entry of the pcd file. 

~binary (bool, default: false)

Output the pcd file in binary form. 

~compressed (bool, default: false)

In case that binary output format is set, use binary compressed output. 

Examples

Subscribe to the /velodyne/pointcloud2 topic and save each message in the current directory. File names look like 1285627014.833100319.pcd, the exact names depending on the message time stamps.

 $ rosrun pcl_ros pointcloud_to_pcd input:=/velodyne/pointcloud2

Set the prefix parameter in the current namespace, save messages to files with names like /tmp/pcd/vel_1285627015.132700443.pcd.

 $ rosrun pcl_ros pointcloud_to_pcd input:=/my_cloud _prefix:=/tmp/pcd/vel_

3 总结

pcl_ros做了在ros中使用pcl库的一些基础功能接口，可以使得直接通过nodelet进行调用pcl的一些功能。

pcl_conversions 做了一些ros的消息类型与pcl消息类型的转换。

ros中并没有定义pcl的数据类型，所以，只要是pcl相关的数据类型，都是pcl自身的，没有ros中定义的，ros只是定义了一些转换函数。这块我之前一直有疑问。