本篇文章记录了下 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只是定义了一些转换函数。这块我之前一直有疑问。