将 ros PointCloud2格式数据转为livox CustomMsg格式
- 前言
- 点云格式
- PointCloud2 点云格式
- livox CustomMsg 点云格式
- 将 ros PointCloud2格式数据转为livox CustomMsg格式
- 测试
前言
览沃科技有限公司（Livox）成立于2016年。为了革新激光雷达行业，Livox致力于提供高性能、低成本的激光雷达传感器。通过降低使用门槛和生产成本，Livox将激光雷达技术集成到更多产品和应用之中，从而为自动驾驶、智慧城市、测绘、移动机器人等行业带来创新性改变。Livox产品已销往包括美国、加拿大、中国、日本和欧盟在内的 26 个国家和地区。
面向智能移动机器人市场，Livox 推出最新一代 3D 激光雷达 Mid-360，开启混合固态激光雷达 360° 立体感知新篇章。凭借小巧体积，Mid-360 的安装布置更加灵活。同时，Mid-360 充分考虑了移动机器人对导航、避障等升维感知的需求，兼容室内外场景，赋能移动机器人进入空间智能感知新时代。 但是这款产品官网一直购买不到， 本篇博客 在 gazebo 中 可实现对该激光雷达的仿真 。
传统激光雷达普遍采用机械扫描方式，扫描路径随时间重复。而Livox 激光雷达采用了独特的扫描⽅式，扫描路径不会重复。在非重复扫描方式中，视场中被激光照射到的区域面积会随时间增大，这意味着视场覆盖率随时间推移而显著提高，可减小视场内物体被漏检的概率，有助于探测视场中的更多细节。
下图中给出了一个直观的例子。（a）图由于采用了非重复扫描方式，随着时间的积累，视场覆盖率逐渐升高。而（b）图，由于扫描每次都是重复的，视场覆盖率几乎没有提升。
gazebo中自带的laser插件是上图中b的方式，并不能代表livox的这种雷达。
livox 的仿真功能包(livox_laser_simulation)中 提供了 多种半固体的雷达仿真模型，可以在gazebo中进行该雷达的仿真与其扫描方式（非重复式扫描）点云发布。
但是经过测试发现 livox_laser_simulation 雷达仿真功能包发布的是 ros PointCloud2 点云格式数据。
而现在针对livox雷达的激光slam算法用的是 livox CustomMsg 点云格式数据，例如 Fast-Lio2 、Faster-Lio 、Lio-livox等。所以想在gazebo中仿真这些算法，需要将 ros PointCloud2格式数据转为livox CustomMsg格式 。
点云格式
PointCloud2 点云格式
PointCloud2 是ros的一种点云格式
具体官方数据 http://docs.ros.org/en/jade/api/sensor_msgs/html/msg/PointCloud2.html
std_msgs/Header header
uint32 seq
time stamp
string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
uint8 INT8=1
uint8 UINT8=2
uint8 INT16=3
uint8 UINT16=4
uint8 INT32=5
uint8 UINT32=6
uint8 FLOAT32=7
uint8 FLOAT64=8
string name
uint32 offset
uint8 datatype
uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense
一个仿真的点云消息长这样
header: // 点云的头信息
seq: 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: [ .......................................................... ] // Actual point data, size is (row_step*height)
is_dense: True // 没有非法数据点
livox CustomMsg 点云格式
CustomMsg 是 livox雷达的 专有的点云格式，其格式如下：
Header header # ROS standard message header
uint64 timebase # The time of first point
uint32 point_num # Total number of pointclouds
uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data
上述自定义数据包中的自定义点云（CustomPoint）格式 :
uint32 offset_time # offset time relative to the base time
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar
- line
最后一行line是激光雷达扫描的线数（livox avia为6线），从原始扫描点云可以看出，后期可以按线来对原始点处理、筛选 - tag
tag: 主要指示多回波信息及噪点信息。是一个二进制表达，标记信息的格式如下：bit7 bit6 bit5 bit4 bit3 bit2 bit1 bit0
bit7 bit6 保留位
bit5 bit4 回波序号 : 00: 第 0 个回波 01: 第 1 个回波 10: 第 2 个回波 11: 第 3 个回波 由于Livox Avia 采用同轴光路，即使外部无被测物体，其内部的光学系统也会产生一个回波，该回波记为第 0个回波。随后，若激光出射方向存在可被探测的物体，则最先返回系统的激光回波记为第 1 个回波，随后为第 2个回波，以此类推。如果被探测物体距离过近（例如 1.5 m），第 1 个回波将会融合到第 0 个回波里，该回波记为第 0 个回波。
bit3 bit2 基于强度的点属性： 00：正常点 01：回波能量噪点置信度高 10：回波能量噪点置信度中 11：保留位
基于回波能量强度判断采样点是否为噪点。通常情况下，激光光束受到类似灰尘、雨雾、雪等干扰产生的噪点的回波能量很小。目前按照回波能量强度大小将噪点置信度分为二档：01表示回波能量很弱，这类采样点有较高概率为噪点，例如灰尘点；10表示回波能量中等，该类采样点有中等概率为噪点，例如雨雾噪点。噪点置信度越低，说明该点是噪点的可能性越低。bit1 bit0 基于空间位置的点属性 : 00：正常点 01：空间噪点置信度高 10：空间噪点置信度中 11：空间噪点置信度低
基于采样点的空间位置判断是否为噪点。例如，激光探测测距仪在测量前后两个距离十分相近的物体时，两个物体之间可能会产生拉丝状的噪点。目前按照不同的噪点置信度分为三档，噪点置信度越低，说明该点是噪点的可能性越低。
-
offset_time
时间戳
点云数据及 IMU 数据中都包含时间戳信息。 -
reflectivity
目标反射率
目标反射率：以 0 至 255 表示。其中 0 至 150 对应反射率介于 0 至 100% 的漫散射物体；而 151 至255 对应全反射物体。 -
x y z
坐标信息：Livox Avia 的坐标信息可表示为直角坐标（x, y, z）或球坐标（r, θ, φ）。如果前方无被探测物体或者被探测物体超出量程范围（例如 600 m），在直角坐标系下，点云输出为（0，0，0）；在球坐标系下，点云输出为（0，θ, φ）。
将 ros PointCloud2格式数据转为livox CustomMsg格式
为了使 livox_laser_simulation 功能包可以输出 CustomMsg 格式数据用于 fast-lio仿真，需要对其代码进行修改。主要是livox_laser_simulation/src/livox_points_plugin.cpp 这个文件。
重新编写这个函数
void LivoxPointsPlugin::OnNewLaserScans()
前面的这些可以不变
if (rayShape)
{
std::vector<std::pair<int, AviaRotateInfo>> points_pair;
InitializeRays(points_pair, rayShape);
rayShape->Update();
msgs::Set(laserMsg.mutable_time(), world->SimTime());
msgs::LaserScan *scan = laserMsg.mutable_scan();
InitializeScan(scan);
SendRosTf(parentEntity->WorldPose(), world->Name(), raySensor->ParentName());
auto rayCount = RayCount();
auto verticalRayCount = VerticalRayCount();
auto angle_min = AngleMin().Radian();
auto angle_incre = AngleResolution();
auto verticle_min = VerticalAngleMin().Radian();
auto verticle_incre = VerticalAngleResolution();
声明要发布的 custom格式的livox 点云
livox_ros_driver::CustomMsg pp_livox;//声明要发布的 custom格式的livox 点云
赋值点云帧头
// 赋值点云帧头
pp_livox.header.stamp = ros::Time::now();//时间
pp_livox.header.frame_id = "livox";//坐标系
int count = 0;
用 high_resolution_clock 记录 起始时间
boost::chrono::high_resolution_clock::time_point start_time = boost::chrono::high_resolution_clock::now();
遍历每个点
for (auto &pair : points_pair)
{
获得点的距离
auto range = rayShape->GetRange(pair.first);
获得点的强度
auto intensity = rayShape->GetRetro(pair.first);
最大最小距离限制
if (range >= RangeMax()) {
range = 0;
} else if (range <= RangeMin()) {
range = 0;
}
将极坐标转为x y z
auto rotate_info = pair.second;
// 将极坐标转为x y z
ignition::math::Quaterniond ray;
ray.Euler(ignition::math::Vector3d(0.0, rotate_info.zenith, rotate_info.azimuth));
auto axis = ray * ignition::math::Vector3d(1.0, 0.0, 0.0);
auto point = range * axis;
声明一个 Custom 点
livox_ros_driver::CustomPoint p;//声明一个 Custom 点
赋值坐标
p.x = point.X();
p.y = point.Y();
p.z = point.Z();
赋值强度
p.reflectivity = intensity;
计算当前点时间到起始时间差
boost::chrono::high_resolution_clock::time_point end_time = boost::chrono::high_resolution_clock::now();// 当前点时间
boost::chrono::nanoseconds elapsed_time = boost::chrono::duration_cast<boost::chrono::nanoseconds>(end_time - start_time);
赋值偏移时间
p.offset_time = elapsed_time.count();
将当前点存入点云中
pp_livox.points.push_back(p);
// 计点个数的计数器加1
count ++;
}//结束遍历每个点
赋值点云中点的个数
pp_livox.point_num = count;
发布点云
livox_pub.publish(pp_livox);
ros::spinOnce();
}
测试
将功能包进行编译，需要引入 livox_ros_driver 功能包
find_package(catkin REQUIRED COMPONENTS
roscpp
tf
livox_ros_driver # 新增
)
包含头文件
#include <livox_ros_driver/CustomMsg.h>
完成编译后启动仿真环境
产看当前话题名
可以看到这个 /livox/lidar 话题名
查看这个话题信息
rostopic info /livox/lidar
jk-jone@JKKC:~$ rostopic info /livox/lidar
Type: livox_ros_driver/CustomMsg
Publishers:
/gazebo (http://JKKC:38793/)
Subscribers: None
消息类型改成了 livox_ros_driver/CustomMsg
![[原创][R语言]股票分析实战:周级别涨幅趋势的相关性](https://img-blog.csdnimg.cn/direct/f5c2a47ff1db41f79d0f9a1b7e797938.png)
![[极客大挑战 2019]BuyFlag1](https://img-blog.csdnimg.cn/direct/2f855085cff04189ac61ddf169f874e2.png#pic_center)
