激光雷达点云消息参数
1. 2D激光雷达
https://www.jianshu.com/p/b73ccaf19d7e
1.1 如horizontal_laser_2d
header:
seq: 0
stamp:
secs: 1405946671
nsecs: 406303800
frame_id: "horizontal_laser_link"
angle_min: -2.35183119774
angle_max: 2.35183119774
angle_increment: 0.00436332309619
time_increment: 1.73611151695e-05
scan_time: 0.0250000003725
range_min: 0.0230000000447
range_max: 60.0
ranges:
-
echoes: [2.0769999027252197]
-
echoes: [2.065999984741211]
-
- 。。。。。。。
二维激光雷达直接把范围ranges输出出来，因此不需要过多处理。
1.2 2d解析
在cartographer中，将二激光雷达的ranges数据解析为点云格式的代码位于cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc
// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
// 将激光数据包转换为点云格式数据
// 值得看看
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
CHECK_GE(msg.range_min, 0.f);
CHECK_GE(msg.range_max, msg.range_min);
if (msg.angle_increment > 0.f) {
CHECK_GT(msg.angle_max, msg.angle_min);
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
PointCloudWithIntensities point_cloud;
float angle = msg.angle_min;
for (size_t i = 0; i < msg.ranges.size(); ++i) {
const auto& echoes = msg.ranges[i];
if (HasEcho(echoes)) {
const float first_echo = GetFirstEcho(echoes);
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
Eigen::Vector4f point;
point << rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment;
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i];
CHECK(HasEcho(echo_intensities));
point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
angle += msg.angle_increment;
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back()[3];
timestamp += cartographer::common::FromSeconds(duration);
for (Eigen::Vector4f& point : point_cloud.points) {
point[3] -= duration;
}如
}
return std::make_tuple(point_cloud, timestamp);
}
2. 3D激光雷达
2.1 如horizontal_laser_3d
header: // 点云的头信息
seq: 963 // 第963帧数据
stamp: // 时间戳
secs: 1541143772
nsecs: 912011000
frame_id: "/camera_init"
height: 1 // If the cloud is unordered, height is 1 如果cloud 是无序的 height 是 1
width: 852578 //点云的长度
fields: // sensor_msgs/PointField[] fields
-
name: "x"
offset: 0
datatype: 7 // uint8 INT8 = 1
// uint8 UINT8 = 2
// uint8 INT16 = 3
// uint8 UINT16 = 4
// uint8 INT32 = 5
// uint8 UINT32 = 6
// uint8 FLOAT32 = 7
// uint8 FLOAT64 = 8
count: 1
-
name: "y"
offset: 4
datatype: 7
count: 1
-
name: "z"
offset: 8
datatype: 7
count: 1
-
name: "intensity"
offset: 16
datatype: 7
count: 1
is_bigendian: False
point_step: 32 // Length of a point in bytes 一个点占的比特数 （字节）
row_step: 27282496 // Length of a row in bytes 一行的长度占用的比特数
data: [152, 94, 11, 65, 144, 79, 174, 64, .......................................................... ] // Actual point data, size is (row_step*height)
is_dense: True // 没有非法数据点
2.2 3d解析
!!! data 内部包含的是 包含点云的二进制数据流！ 必须要单独解析，直接读取没有任何意义
如何解析需要用python: https://blog.csdn.net/Fourier_Legend/article/details/83656798
但是ros提供了pcl工具可以直接用来处理点云,将ros的点云数据data格式转换为pcl点云格式
在cartographer中的sensor_bridge.cc中含有HandlePointCloud2Message函数，其中的pcl::fromROSMsg
可进行转换
// 3d激光点云数据处理
// sensor_id即传感器消息的话题名称
void SensorBridge::HandlePointCloud2Message( // 点云信息处理，3d的激光数据包走这个函数
const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
// 准备用pcl点云（x,y,z）接一下
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
// 直接调用pcl的库将二进制数据转换为pcl点云数据
pcl::fromROSMsg(*msg, pcl_point_cloud);
carto::sensor::TimedPointCloud point_cloud;
for (const auto& point : pcl_point_cloud) {
point_cloud.emplace_back(point.x, point.y, point.z, 0.f);
}
HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
point_cloud);
}