消息类型
sensor_msgs/PointCloud2
类型如下:
1 | # N维的点云,点的数据以二进制blob形式保存,其layout用fields数组描述 |
PointCloud2的data是序列化后的数据,直接看不到物理意义。只能转为点云1类型
sensor_msgs/PointCloud
类型如下:
1 | Header header |
坐标系和点云的位置关系如下,我们感兴趣的是z,也就是相机坐标系到点云的距离
从sensor_msgs::PointCloud2
类型转为sensor_msgs::PointCloud
,而且获知坐标
1 |
|
消息的发布
意外发现ROS可以直接发布pcl的类型,不必转为ROS的消息类型
1 | typedef pcl::PointCloud<PointT> PointCloudT; |
ros在
pcl_ros
中做了Publisher的多态。使得ros的标准Publisher可以接收pcl::PointCloud<T>
类型的消息,并自动转换成sensor_msgs::PointCloud2
的消息类型,再发布出去。
几种点云类型的转换
点云格式主要有四种,sensor_msgs::PointCloud
,也就是点云1已经弃用,主要用点云2,但是点云1有时还有用。比如点云2转为点云1消息,可看到包含多少个点,rostopic echo --noarr pcl1/points
,结果可以是<array type: geometry_msgs/Point32, length: 256000>
,可看到每个点的坐标值。
sensor_msgs::PointCloud
sensor_msgs::PointCloud2
pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)
pcl::PointCloud
— standard PCL data structure
find_package
里添加sensor_msgs
, pcl_ros
, pcl_conversions
ROS PointCloud2 和 PointCloud的转换
1 | sensor_msgs::PointCloud out_pcl; |
反过来就用sensor_msgs::convertPointCloudToPointCloud2
可以使用point_cloud_converter在点云1和点云2 之间转换
1 | PointCloudConverter initialized to transform from PointCloud (/points_in) to PointCloud2 (/points2_out). |
比如
rosrun point_cloud_converter point_cloud_converter_node points2_in:=/camera/depth/points points_out:=/pcl1
PointCloud2 转 pcl::PCLPointCloud2
1 | sensor_msgs::PointCloud2 cloud; |
pcl::PCLPointCloud2 转 pcl::PointCloud < pcl::PointXYZ >
1 | pcl::PCLPointCloud2 pcl_pc2; |
pcl::PointCloud < pcl::PointXYZ > 转 sensor_msgs::PointCloud2
1 |
|
反过来就是fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T>&)
#include <sensor_msgs/point_cloud2_iterator.h>
参考: pcl_conversions