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 **/
// 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的发布器 发布出去，发出的数据类型为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:
is the bag file name to read.
is the topic in the bag file containing messages to save.
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
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:
is the (required) file name to read.
is the (optional) number of seconds to sleep between messages. If 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只是定义了一些转换函数。这块我之前一直有疑问。