前言
提示:这里可以添加本文要记录的大概内容:
本人在ROS
环境下使用Ouster
激光雷达获取的点云做检测算法,想要获取点云中的每个点所在的线数ring
信息。首先通过查找资料,velodyne
的激光雷达驱动程序可以直接获取线数ring
信息,但是其他家的激光雷达驱动大部分都不可以。
本着爱钻研的习惯,研究了Ouster
激光雷达的ROS
驱动程序,发现其定义的点的数据类型中是包含ring
信息的,但是在转化为ROS
消息的过程中损失掉了。
想要不损失ring
信息,必须接收点云定义的点的数据类型
和驱动程序的定义的点的数据类型
一样。
一、Ouster驱动程序定义的Point的数据类型
Ouster激光雷达的驱动程序安装过程,我就不展开说了,参考以下链接:
https://ouster.atlassian.net/wiki/spaces/SUPPORT/pages/928120883/Ouster+ROS
ROS驱动程序主要使用的功能包是ouster_ros
在ouster_ros
->include
->ouster_ros
->point.h
中定义的数据类型如下,其中uint8_t ring
表示的是激光雷达点的线数信息。
namespace ouster_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t ambient;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros
二、如何接收带有ring
信息的点云
说明:由于经历了驱动程序将点云转化为ROS格式发布,我们要使用
pcl::fromROSMsg()
函数接收带有ring
信息的点云,需要首先定义和驱动程序一样的点类型。如下图所示:
1. 定义自己的点类型
// (1) 定义自己的点类型
struct EIGEN_ALIGN16 MyPoint {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t ambient;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
// (2)向PCL点云库注册自己的点类型
POINT_CLOUD_REGISTER_POINT_STRUCT(MyPoint,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint8_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)
2. 接收程序
/*
回调函数定义...略,假设回调函数入口参数是 (const sensor_msgs::PointCloud2ConstPtr &in_cloud_ptr)
*/
// 定义接收变量
pcl::PointCloud<MyPoint> laserCloudIn;
// 将ROS格式点云转化为PCL格式
pcl::fromROSMsg(*in_cloud_ptr, laserCloudIn);
// 打印每个点的ring信息
for (size_t i = 0; i < laserCloudIn.points.size(); i++)
{
// 注意这里要使用printf函数打印输出查看,使用cout终端打印会出错
printf("%d \n", laserCloudIn.points[i].ring );
}