激光雷达
简介
英文名为Light Detection and Ranging，简称LDR。现介绍EAI的YDLIDAR X2激光雷达产品。这是属于机械旋转式激光雷达，主要可用于机器人避障，ros学习方面。下图可观测其主要参数：
X2主要沿用UART串口进行通信，用户可通过物理接口来连接系统获取点云信息/设备信息等。
要关注波特率等信息，写代码时需要。
X2自带电机调速功能的电机驱动器，外设可通过接口中的M_CTR管脚输入控制信号来对
X2的电机进行控制。MCTR为电机速度控制信号，可电压调速，也可以PWM波调试，电压越
高/PWM占空比越大，电机转速越高。
值得注意的是X2内部已经定义了自身坐标系，为极坐标系。这与一般的笛卡尔坐标系不同。
驱动的launch文件
X2的ros驱动文件中launch文件是：
<launch>
<node name="ydlidar_lidar_publisher" pkg="ydlidar_ros_driver" type="ydlidar_ros_driver_node" output="screen" respawn="false" >
<!-- string property -->
<param name="port" type="string" value="/dev/ydlidar"/>
<param name="frame_id" type="string" value="laser_frame"/>
<param name="ignore_array" type="string" value=""/>
<!-- int property -->
<param name="baudrate" type="int" value="115200"/>
<!-- 0:TYPE_TOF, 1:TYPE_TRIANGLE, 2:TYPE_TOF_NET -->
<param name="lidar_type" type="int" value="1"/>
<!-- 0:YDLIDAR_TYPE_SERIAL, 1:YDLIDAR_TYPE_TCP -->
<param name="device_type" type="int" value="0"/>
<param name="sample_rate" type="int" value="3"/>
<param name="abnormal_check_count" type="int" value="4"/>
<!-- bool property -->
<param name="resolution_fixed" type="bool" value="true"/>
<param name="auto_reconnect" type="bool" value="true"/>
<param name="reversion" type="bool" value="false"/>
<param name="inverted" type="bool" value="true"/>
<param name="isSingleChannel" type="bool" value="true"/>
<param name="intensity" type="bool" value="false"/>
<param name="support_motor_dtr" type="bool" value="true"/>
<param name="invalid_range_is_inf" type="bool" value="false"/>
<param name="point_cloud_preservative" type="bool" value="false"/>
<!-- float property -->
<param name="angle_min" type="double" value="-180" />
<param name="angle_max" type="double" value="180" />
<param name="range_min" type="double" value="0.1" />
<param name="range_max" type="double" value="12.0" />
<!-- frequency is invalid, External PWM control speed -->
<param name="frequency" type="double" value="10.0"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4"
args="0.0 0.0 0.2 0.0 0.0 0.0 /base_footprint /laser_frame 40" />
</launch>
在这里尤为关注几点：
<param name="angle_min" value="-180"/>
<param name="angle_max" value="180"/>
这两个是控制雷达发布发布完整一圈（360°）数据，关键在于“发布”。 `
` 改成这样是只发布前方180°的数据，但雷达本身还是在扫描360度的。此类产品是电机带着激光头 → 360°旋转 → 持续采样，这种设计让其无法只扫描180度。<param name="inverted" value="true"/>
这段代码块是负责控制电机正反转的。设置inverted = true就是电机反转。false就是不反转，按驱动默认方向输出点云。功能是：激光点云角度反转（0° ↔ 360° 翻转） 修正点云镜像 / 错位问题
<param name="reversion" value="false"/>
这段代码是控制雷达是正装还是反着装的。reversion = true就是反装。reversion = false就是正装。
<node pkg="tf" type="static_transform_publisher"
args="0.0 0.0 0.2 0.0 0.0 0.0 /base_footprint /laser_frame 40" />
还有这个非常关键的TF坐标系的描述，雷达在机器人上方 0.2m。对应的是x y z roll pitch yaw 父坐标系 子坐标系 发布频率。roll就是翻滚吗，可以理解为飞机翻滚，机身由平衡到斜向下的状态变化；pitch是偏航，就是开四轮车打方向盘偏航的意思；而yaw俯仰角，就是飞机平衡状态变成向上翘起，或者鬼火少年翘车头一样。
驱动主函数
//
// The MIT License (MIT)
//
// Copyright (c) 2020 EAIBOT. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
//#include "ydlidar_ros_driver/LaserFan.h"
#include "std_srvs/Empty.h"
#include "src/CYdLidar.h"
#include "ydlidar_config.h"
#include <limits> // std::numeric_limits
#define SDKROSVerision "1.0.2"
CYdLidar laser;
bool stop_scan(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res) {
ROS_DEBUG("Stop scan");
return laser.turnOff();
}
bool start_scan(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res) {
ROS_DEBUG("Start scan");
return laser.turnOn();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "ydlidar_ros_driver");
ROS_INFO("YDLIDAR ROS Driver Version: %s", SDKROSVerision);
ros::NodeHandle nh;
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
ros::Publisher pc_pub = nh.advertise<sensor_msgs::PointCloud>("point_cloud",
1);
// ros::Publisher laser_fan_pub =
// nh.advertise<ydlidar_ros_driver::LaserFan>("laser_fan", 1);
ros::NodeHandle nh_private("~");
std::string str_optvalue = "/dev/ydlidar";
nh_private.param<std::string>("port", str_optvalue, "/dev/ydlidar");
///lidar port 雷达的串口设备名字
laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(),
str_optvalue.size());
///ignore array
nh_private.param<std::string>("ignore_array", str_optvalue, "");
laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(),
str_optvalue.size());
std::string frame_id = "laser_frame";
nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
//////////////////////int property/////////////////
/// lidar baudrate 雷达的波特率
int optval = 230400;
nh_private.param<int>("baudrate", optval, 230400);
laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
/// tof lidar
optval = TYPE_TRIANGLE;
nh_private.param<int>("lidar_type", optval, TYPE_TRIANGLE);
laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
/// device type 设备类型，这里也需要改
optval = YDLIDAR_TYPE_SERIAL;
nh_private.param<int>("device_type", optval, YDLIDAR_TYPE_SERIAL);
laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
/// sample rate 采样频率，这里要关注
optval = 9;
nh_private.param<int>("sample_rate", optval, 9);
laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
/// abnormal count
optval = 4;
nh_private.param<int>("abnormal_check_count", optval, 4);
laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
//intensity bit count
optval = 10;
nh_private.param<int>("intensity_bit", optval, 10);
laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
//////////////////////bool property/////////////////
/// fixed angle resolution
bool b_optvalue = false;
nh_private.param<bool>("resolution_fixed", b_optvalue, true);
laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
/// rotate 180
nh_private.param<bool>("reversion", b_optvalue, true);
laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
/// Counterclockwise
nh_private.param<bool>("inverted", b_optvalue, true);
laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
b_optvalue = true;
nh_private.param<bool>("auto_reconnect", b_optvalue, true);
laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
/// one-way communication
b_optvalue = false;
nh_private.param<bool>("isSingleChannel", b_optvalue, false);
laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
/// intensity
b_optvalue = false;
nh_private.param<bool>("intensity", b_optvalue, false);
laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
/// Motor DTR
b_optvalue = false;
nh_private.param<bool>("support_motor_dtr", b_optvalue, false);
laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
//debug
b_optvalue = false;
nh_private.param<bool>("debug", b_optvalue, false);
laser.setEnableDebug(b_optvalue);
//////////////////////float property/////////////////
/// unit: ° 注意雷达的角度范围
float f_optvalue = 180.0f;
nh_private.param<float>("angle_max", f_optvalue, 180.f);
laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
f_optvalue = -180.0f;
nh_private.param<float>("angle_min", f_optvalue, -180.f);
laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
/// unit: m 注意雷达的角度范围
f_optvalue = 16.f;
nh_private.param<float>("range_max", f_optvalue, 16.f);
laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
f_optvalue = 0.1f;
nh_private.param<float>("range_min", f_optvalue, 0.1f);
laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
/// unit: Hz
f_optvalue = 10.f;
nh_private.param<float>("frequency", f_optvalue, 10.f);
laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));
bool invalid_range_is_inf = false;
nh_private.param<bool>("invalid_range_is_inf", invalid_range_is_inf,
invalid_range_is_inf);
bool point_cloud_preservative = false;
nh_private.param<bool>("point_cloud_preservative", point_cloud_preservative,
point_cloud_preservative);
ros::ServiceServer stop_scan_service = nh.advertiseService("stop_scan",
stop_scan);
ros::ServiceServer start_scan_service = nh.advertiseService("start_scan",
start_scan);
//initialize SDK and LiDAR
bool ret = laser.initialize();
if (ret)
{
//设置GS工作模式
int i_v = 0;
nh_private.param<int>("m1_mode", i_v, 0);
laser.setWorkMode(i_v, 0x01);
i_v = 0;
nh_private.param<int>("m2_mode", i_v, 0);
laser.setWorkMode(i_v, 0x02);
i_v = 1;
nh_private.param<int>("m3_mode", i_v, 1);
laser.setWorkMode(i_v, 0x04);
//Start the device scanning.
ret = laser.turnOn();
}
else
{
ROS_ERROR("%s\n", laser.DescribeError());
}
ros::Rate r(30);
while (ret && ros::ok()) {
LaserScan scan;
if (laser.doProcessSimple(scan)) {
sensor_msgs::LaserScan scan_msg;
sensor_msgs::PointCloud pc_msg;
// ydlidar_ros_driver::LaserFan fan;
ros::Time start_scan_time;
start_scan_time.sec = scan.stamp / 1000000000ul;
start_scan_time.nsec = scan.stamp % 1000000000ul;
scan_msg.header.stamp = start_scan_time;
scan_msg.header.frame_id = frame_id;
pc_msg.header = scan_msg.header;
// fan.header = scan_msg.header;
scan_msg.angle_min = (scan.config.min_angle);
scan_msg.angle_max = (scan.config.max_angle);
scan_msg.angle_increment = (scan.config.angle_increment);
scan_msg.scan_time = scan.config.scan_time;
scan_msg.time_increment = scan.config.time_increment;
scan_msg.range_min = (scan.config.min_range);
scan_msg.range_max = (scan.config.max_range);
// fan.angle_min = (scan.config.min_angle);
// fan.angle_max = (scan.config.max_angle);
// fan.scan_time = scan.config.scan_time;
// fan.time_increment = scan.config.time_increment;
// fan.range_min = (scan.config.min_range);
// fan.range_max = (scan.config.max_range);
int size = (scan.config.max_angle - scan.config.min_angle) /
scan.config.angle_increment + 1;
scan_msg.ranges.resize(size,
invalid_range_is_inf ? std::numeric_limits<float>::infinity() : 0.0);
scan_msg.intensities.resize(size);
pc_msg.channels.resize(2);
int idx_intensity = 0;
pc_msg.channels[idx_intensity].name = "intensities";
int idx_timestamp = 1;
pc_msg.channels[idx_timestamp].name = "stamps";
for (size_t i = 0; i < scan.points.size(); i++) {
int index = std::ceil((scan.points[i].angle - scan.config.min_angle) /
scan.config.angle_increment);
if (index >= 0 && index < size) {
if (scan.points[i].range >= scan.config.min_range) {
scan_msg.ranges[index] = scan.points[i].range;
scan_msg.intensities[index] = scan.points[i].intensity;
}
}
if (point_cloud_preservative ||
(scan.points[i].range >= scan.config.min_range &&
scan.points[i].range <= scan.config.max_range)) {
geometry_msgs::Point32 point;
point.x = scan.points[i].range * cos(scan.points[i].angle);
point.y = scan.points[i].range * sin(scan.points[i].angle);
point.z = 0.0;
pc_msg.points.push_back(point);
pc_msg.channels[idx_intensity].values.push_back(scan.points[i].intensity);
pc_msg.channels[idx_timestamp].values.push_back(i * scan.config.time_increment);
}
// fan.angles.push_back(scan.points[i].angle);
// fan.ranges.push_back(scan.points[i].range);
// fan.intensities.push_back(scan.points[i].intensity);
}
scan_pub.publish(scan_msg);
pc_pub.publish(pc_msg);
// laser_fan_pub.publish(fan);
} else {
ROS_ERROR("Failed to get Lidar Data");
}
r.sleep();
ros::spinOnce();
}
laser.turnOff();
ROS_INFO("[YDLIDAR INFO] Now YDLIDAR is stopping .......");
laser.disconnecting();
return 0;
}
`
在ros中需要关注几点：
-
串口权限不对 → 雷达打不开
-
波特率不对 → 读不到数据
-
reversion /inverted 没设对 → 点云方向乱
-
angle_max/min 不对 → 点云缺一半
-
frame_id 不对 → RViz 看不到点云
1. 串口设备名（最容易错）
std::string str_optvalue = "/dev/ydlidar";
nh_private.param<std::string>("port", str_optvalue, "/dev/ydlidar");
要改：你的雷达串口名，比如 /dev/ttyUSB0、/dev/ttyACM0
2. 波特率（必须和雷达一致）
int optval = 230400;
nh_private.param<int>("baudrate", optval, 230400);
要改：看你雷达手册！常见：115200、230400、460800、921600
3. 雷达类型
optval = TYPE_TRIANGLE; // 三角法
// 或 TYPE_TOF = 飞行时间（思岚、镭神等TOF雷达）
要改：看你雷达是三角法还是TOF
4. 你最关心的两个参数：倒装 & 反转
nh_private.param<bool>("reversion", b_optvalue, true); // 倒装180度
nh_private.param<bool>("inverted", b_optvalue, true); // 旋转方向反转
你要改：
- reversion=true = 雷达上下倒装
- inverted=true = 扫描方向反转
5. 角度范围（你的雷达不一定是 360 度）
f_optvalue = 180.0f;
nh_private.param<float>("angle_max", f_optvalue, 180.f);
f_optvalue = -180.0f;
nh_private.param<float>("angle_min", f_optvalue, -180.f);
你要改：你的雷达角度：0360 或 -9090 等
6. 距离范围（最远测多少米）
f_optvalue = 16.f; // 最大16米
nh_private.param<float>("range_max", f_optvalue, 16.f);
✅ 你要改：你的雷达最大测距：12m、20m、30m…
7. 扫描频率
f_optvalue = 10.f; // 10Hz
nh_private.param<float>("frequency", f_optvalue, 10.f);
✅ 你要改：你的雷达支持 5Hz/10Hz/20Hz 等
8. frame_id 坐标系（RViz 显示用）
std::string frame_id = "laser_frame";
这段代码的数据流向是这样的：
1. [ 雷达硬件 ] 通过（串口发送原始角度、距离）
2.到[ 驱动SDK (CYdLidar) ] （doProcessSimple）
3.到[ 主函数代码 ]（解析、填充、格式化）
4.到[ ROS消息 (LaserScan) ] （publish发布）
5.到[ RViz / 导航 / SLAM ]
驱动代码库在此处： ydlidar_ros_driver/src/ydlidar_ros_driver.cpp at master · YDLIDAR/ydlidar_ros_driver
以初学者的视角而言，在阅读驱动代码的时候，必须要想几点，从代码中能学到什么，如果面试时扯这个驱动代码，面试会问什么内容？
这个驱动的功能是把雷达数据转换成ROS的LaserScan并发布。 LaserScan 结构是-
- ranges[] //ranges 是按角度排列的距离数组，雷达往某个方向打激光 → 撞到物体 → 返回距离
//ranges[0] = 1.0 → 前方1米有障碍物 // ranges[1] = 2.5 → 稍微偏一点方向2.5米（不同方向有不同的障碍物） //
-
angle_min //angle_min 是扫描起始角度
-
angle_increment//每两个点之间的角度间隔
-
frame_id//这个雷达数据属于哪个坐标系
bool ret = laser.initialize();
这段代码就是，启动雷达。完成了硬件初始化 + 通信初始化 + 状态初始化。 initialize函数用于初始化雷达SDK，包括打开通信接口、设置波特率、与雷达设备进行握手，并确认设备状态正常；返回值表示初始化是否成功，用于控制驱动是否进入数据采集循环。 而lasser一般是雷达的sdk对象。 代码主循环关注：
while (ret && ros::ok())
这表示：只要ROS没关 + 雷达正常 → 一直循环 主循环要做的事是： 1.持续从雷达SDK获取一帧扫描数据， 2.然后将原始的极坐标数据转换为ROS标准的LaserScan消息， 包括角度映射和距离填充， 同时可选生成点云数据， 3. 最后发布到/scan话题供SLAM和导航使用。 整个循环可以分成 5步 Step 1：从雷达拿数据 if (laser.doProcessSimple(scan))//持续从雷达SDK获取一帧扫描数据，
Step 2：创建ROS消息
sensor_msgs::LaserScan scan_msg;
sensor_msgs::PointCloud pc_msg;
LaserScan用于导航和slam建图，PointCloud（点云）用于可视化以及感知
Step 3：填充“头信息”
scan_msg.header.stamp = start_scan_time; scan_msg.header.frame_id = frame_id; //时间戳和坐标系
Step 4：配置扫描参数
scan_msg.angle_min = ...
scan_msg.angle_max = ...
scan_msg.angle_increment = ...
Step 5：初始化数组
确定有多少个角度点
size = (max_angle - min_angle) / increment + 1
也就是：
一圈要分多少“格子”
先填默认值
inf 或 0
表示：
“还没测到数据”
八、Step 6：核心循环（最重要 ）
for (size_t i = 0; i < scan.points.size(); i++)
遍历每一个激光点
子步骤1：算“放在哪个角度格子”
index = ceil((angle - min_angle) / increment)
本质是：把连续角度 → 映射到数组下标
举例：
-90° → index 0
-89° → index 1
子步骤2：填 ranges
scan_msg.ranges[index] = range;
子步骤3：生成点云
x = r * cos(angle)
y = r * sin(angle)
其实就是：极坐标 → 笛卡尔坐标
这一步就是：LaserScan → PointCloud
九、Step 7：发布数据
scan_pub.publish(scan_msg);
pc_pub.publish(pc_msg);
把数据发出去：
/scan
SLAM 导航
/point_cloud
给： RViz显示/ 3D处理
十、Step 8：控制循环节奏
r.sleep();//控制频率（比如10Hz）
ros::spinOnce();//处理ROS回调（比如服务/参数）
思考面试的点：
1. 为什么 ROS 消息传递要用 & 引用？
答： 消息数据大，引用不拷贝数据，效率更高，不占内存。
2. vector 和普通数组有什么区别？
答： vector 是动态数组，大小可以自动扩展，驱动里存点云、距离数组全靠它。
3. const 能修饰什么？
答： 变量、函数参数、指针，意思都是只读不可修改。
4. TF错误会发生什么（高频问题）
如果TF设置错误，会导致：
- 雷达数据方向错误（镜像）
- 地图旋转或错位
- SLAM失败
X2 雷达SDK： YDLidar-SDK/src/ydlidar_sdk.cpp at master · YDLIDAR/YDLidar-SDK
//
// The MIT License (MIT)
//
// Copyright (c) 2019-2020 EAIBOT. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <sstream>
#include "ydlidar_sdk.h"
#include "CYdLidar.h"
#include "ydlidar_config.h"
YDLidar *lidarCreate() {
CYdLidar *lidar = new CYdLidar();
YDLidar *instance = new YDLidar;
instance->lidar = NULL;
instance->lidar = (void *)lidar;
return instance;
}
void lidarDestroy(YDLidar **lidar) {
if (lidar == NULL || *lidar == NULL) {
return;
}
CYdLidar *drv = static_cast<CYdLidar *>((*lidar)->lidar);
if (drv) {
delete drv;
drv = NULL;
}
(*lidar)->lidar = NULL;
delete *lidar;
*lidar = NULL;
return;
}
bool setlidaropt(YDLidar *lidar, int optname, const void *optval, int optlen) {
if (lidar == NULL || lidar->lidar == NULL || optval == NULL) {
return false;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->setlidaropt(optname, optval, optlen);
}
return false;
}
bool getlidaropt(YDLidar *lidar, int optname, void *optval, int optlen) {
if (lidar == NULL || lidar->lidar == NULL || optval == NULL) {
return false;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->getlidaropt(optname, optval, optlen);
}
return false;
}
void GetSdkVersion(char *version) {
strcpy(version, YDLIDAR_SDK_VERSION_STR);
}
bool initialize(YDLidar *lidar) {
if (lidar == NULL || lidar->lidar == NULL) {
return false;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->initialize();
}
return false;
}
void GetLidarVersion(YDLidar *lidar, LidarVersion *version) {
if (lidar == NULL || lidar->lidar == NULL || version == NULL) {
return;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
drv->GetLidarVersion(*version);
}
}
bool turnOn(YDLidar *lidar) {
if (lidar == NULL || lidar->lidar == NULL) {
return false;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->turnOn();
}
return false;
}
bool doProcessSimple(YDLidar *lidar, LaserFan *outscan) {
if (lidar == NULL || lidar->lidar == NULL || outscan == NULL) {
return false;
}
LaserFanDestroy(outscan);
outscan->npoints = 0;
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
LaserScan scan;
bool ret = drv->doProcessSimple(scan);
outscan->config = scan.config;
outscan->stamp = scan.stamp;
outscan->npoints = scan.points.size();
outscan->points = (LaserPoint *)malloc(sizeof(LaserPoint) * outscan->npoints);
std::copy(scan.points.begin(), scan.points.end(), outscan->points);
return ret;
}
return false;
}
bool turnOff(YDLidar *lidar) {
if (lidar == NULL || lidar->lidar == NULL) {
return false;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->turnOff();
}
return false;
}
void disconnecting(YDLidar *lidar) {
if (lidar == NULL || lidar->lidar == NULL) {
return;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
drv->disconnecting();
}
}
const char *DescribeError(YDLidar *lidar) {
char const *value = "";
if (lidar == NULL || lidar->lidar == NULL) {
return value;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->DescribeError();
}
return value;
}
void os_init() {
ydlidar::os_init();
}
bool os_isOk() {
return ydlidar::os_isOk();
}
void os_shutdown() {
ydlidar::os_shutdown();
}
int lidarPortList(LidarPort *ports) {
if (ports == NULL) {
return 0;
}
memset(ports, 0, sizeof(LidarPort));
std::map<std::string, std::string> lists = ydlidar::lidarPortList();
std::map<std::string, std::string>::iterator it;
int i = 0;
for (it = lists.begin(); it != lists.end(); it++) {
string_t port;
memset(&port, 0, sizeof(string_t));
if (i < sizeof(LidarPort) / sizeof(string_t)) {
memcpy(ports->port[i].data, it->second.c_str(), it->second.size());
}
i++;
}
return i;
}
- doProcessSimple（很重要）
bool doProcessSimple(YDLidar *lidar, LaserFan *outscan) {
if (lidar == NULL || lidar->lidar == NULL || outscan == NULL) {
return false;// 这是一个防御性编程，对空指针检查，用于确保输入参数合法
//这是一个防御性编程，对空指针检查，用于确保输入参数合法。如果雷达对象、内部实现或输出指针为空，
// 函数会直接返回false，避免出现空指针访问导致程序崩溃。
}
LaserFanDestroy(outscan);//释放旧 的点云内存
outscan->npoints = 0;//点数归零
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);//“从雷达句柄里，把底层驱动对象取出来，并转//换成正确的类型。”
//`lidar` = 指针的地址
//`*lidar` = 取出外面那一层 → **真正的雷达对象指针（YDLidar*）**
//`->` 是 指针访问成员变量
//(*lidar)->lidar 从雷达实例里，取出内部保存的底层驱动指针
//CYdLidar *drv = 是创建一个 CYdLidar 类型的驱动指针 drv，让它指向转换后的底层驱动对象。
//static_cast<CYdLidar*> 是安全类型转换，**把 void* 通用指针 → 强转成我们真正的驱动类指针 //CYdLidar***
if (drv) {//如果驱动指针有效，才继续执行
LaserScan scan;
bool ret = drv->doProcessSimple(scan);//
//调用底层驱动，**从雷达硬件读取一帧数据**，放到 `scan` 里。
outscan->config = scan.config;// 复制配置：角度、距离、频率
outscan->stamp = scan.stamp;//// 复制时间戳
outscan->npoints = scan.points.size();//复制点
outscan->points = (LaserPoint *)malloc(sizeof(LaserPoint) * outscan->npoints);
//根据点的数量，申请一块空间存所有点。
std::copy(scan.points.begin(), scan.points.end(), outscan->points);
//把所有点云数据复制过去，如角度，距离，信号强度
return ret;
}
return false;
}
//【安全检查】→【清空旧数据】→【调用底层读数据】→【拷贝数据给上层】→【返回成功/失败】
2. initialize()//硬件初始化
bool initialize(YDLidar *lidar) {
if (lidar == NULL || lidar->lidar == NULL) {
return false;
}
CYdLidar *drv = static_cast<CYdLidar *>(lidar->lidar);
if (drv) {
return drv->initialize();
}
return false;
}
3. turnOn / turnOff（控制雷达开始/停止扫描）
4。 setlidaropt / getlidaropt（配置驱动参数）
bool setlidaropt(YDLidar *lidar, int optname, const void *optval, int optlen)
- 函数作用给雷达设置各种配置：串口、波特率、角度、反转、采样率……
- 参数含义
lidar：雷达句柄optname：要设置哪个参数（比如波特率、端口、reversion）optval：参数的值（比如 230400、true、false）optlen：参数数据长度
5. lidarPortList（工程工具）
- 功能：获取可用串口列表
- 参数：
ports用来存放找到的串口名字 - 返回值：找到多少个串口
if (ports == NULL) {
return 0;
}
- 如果传进来的存储空间是空
- 直接返回 0，防止崩溃
memset(ports, 0, sizeof(LidarPort));
- 把存储串口的空间全部清零
- 避免旧数据干扰
std::map<std::string, std::string> lists = ydlidar::lidarPortList();
- 调用底层函数，获取系统所有串口列表
- 结果存在
lists里
std::map<std::string, std::string>::iterator it;
- 定义一个迭代器（遍历用的工具）
- 用来挨个取出串口名字
int i = 0;
- 计数：找到第几个串口
for (it = lists.begin(); it != lists.end(); it++) {
- 遍历所有找到的串口一个一个拿出来
string_t port;
memset(&port, 0, sizeof(string_t));
定义一个临时变量，存单个串口名， 清零
if (i < sizeof(LidarPort) / sizeof(string_t)) {
memcpy(ports->port[i].data, it->second.c_str(), it->second.size());
}
如果没超过最大数量， 把串口名字复制到结果里
以上介绍完了关于EAI 的Ylidar X2的sdk部分
接下来介绍关于 Ylidar 在ros中的一些开发注意事项。
写代码时，要注意分层设计，
1. 抽象接口层（LidarInterface）
作用 ：定义雷达操作的"标准菜单"
class LidarInterface {
public:
virtual bool initialize(const LidarConfig& config) =
0; // 初始化
virtual bool startScan() =
0; // 开始扫描
virtual bool stopScan() =
0; // 停止扫描
virtual bool getScanData(LidarScanData& scan_data) =
0; // 获取数据
virtual void disconnect() =
0; // 断开连接
};
类比 ：餐厅菜单上写着"宫保鸡丁"、"鱼香肉丝"，不管哪个厨师做，都要有统一的菜名和标准。 好处 ：
- 上层代码只需要知道"有哪些操作"，不需要知道"具体怎么实现"
- 换雷达型号，只要实现相同接口就行
2. 适配器层（YdLidarAdapter）
作用 ：将具体SDK适配成统一接口
class YdLidarAdapter : public LidarInterface {
private:
std::unique_ptr<CYdLidar> lidar_; // YDLidar SDK实例
public:
bool initialize(const LidarConfig& config) override {
// 将统一配置转换为SDK格式
lidar_->setlidaropt(YDLIDAR_OPT_PORT, config.port.
c_str());
lidar_->setlidaropt(YDLIDAR_OPT_BAUDRATE, &config.
baudrate);
// ...
}
bool getScanData(LidarScanData& scan_data) override {
LaserScan ydlidar_scan;
lidar_->doProcessSimple(ydlidar_scan); // 调用SDK获
取数据
convertScanData(ydlidar_scan, scan_data); // 转换为
统一格式
return true;
}
};
类比 ：厨师看菜单（LidarInterface），然后用自己的方法把食材（SDK）做成菜（统一格式数据）。
好处 ：
- 隐藏SDK细节，上层不需要知道SDK的API
- 如果换SDK（比如从YDLidar换成RPLidar），只需要写新的适配器
- 统一数据格式，方便后续处理
3. 工厂层（LidarFactory）
作用 ：根据类型创建对应的适配器
class LidarFactory {
public:
static std::unique_ptr<LidarInterface> createLidar
(LidarType type) {
switch (type) {
case LidarType::YDLIDAR:
return std::make_unique<YdLidarAdapter>();
case LidarType::RPLIDAR:
return std::make_unique<RpLidarAdapter>
(); // 预留
default:
return nullptr;
}
}
};
类比 ：厨房调度员根据顾客点的菜，安排对应的厨师来做。
好处 ：
- 对象创建集中管理，避免到处new
- 调用者只需要说"我要YDLidar"，不需要知道YdLidarAdapter
- 扩展新雷达类型，只需在工厂加一个分支
4. 节点层（ydlidar_node.cpp）
作用 ：ROS节点封装，负责话题发布、参数读取、生命周期管理
int main(int argc, char** argv) {
ros::init(argc, argv, "ydlidar_node");
ros::NodeHandle nh;
// 读取参数
std::string lidar_type;
nh.getParam("lidar_type", lidar_type);
// 工厂创建雷达
auto lidar = LidarFactory::createLidar(lidar_type);
// 初始化雷达
LidarConfig config;
nh.getParam("port", config.port);
lidar->initialize(config);
// 开始扫描并发布话题
ros::Publisher scan_pub = nh.
advertise<sensor_msgs::LaserScan>("scan", 10);
lidar->startScan();
while (ros::ok()) {
LidarScanData scan_data;
lidar->getScanData(scan_data);
// 转换为ROS消息并发布
sensor_msgs::LaserScan msg;
// ... 填充消息字段
scan_pub.publish(msg);
ros::spinOnce();
}
}
类比 ：服务员负责接待顾客、记录订单、端菜上桌。 好处 ：
- 封装ROS相关逻辑，与业务逻辑分离
- 提供参数配置、话题发布等ROS功能
- 管理节点生命周期
Q1：为什么需要工厂模式？
参考答案 ： 工厂模式封装了对象的创建逻辑，调用者只需要知道想要什么类型的对象，不需要知道具体怎么创建。这样可以降低耦合度，提高代码的可扩展性。比如我们项目中新增雷达类型时，只需要添加新的适配器和工厂分支，不需要修改上层代码。
Q2：适配器模式的作用是什么？
参考答案 ： 适配器模式将不兼容的接口转换为统一接口。在我们项目中，不同雷达厂商的SDK接口不同，通过适配器可以把它们转换成统一的 LidarInterface ，让上层代码可以用相同的方式操作不同型号的雷达。
Q3：这种分层设计有什么好处？
参考答案 ： 主要好处是 解耦 和 可扩展 。解耦让各层各司其职，修改一层不会影响其他层；可扩展让新增功能（如支持新雷达）变得简单。此外还有可测试性好、代码复用率高等优点。
共享内存
在ros开发当中，为了避免多次内存拷贝，消耗cpu资源，会有一种共享内存的策略去实现。 // 一般写在node节点上 #include <shm_transport/shm_topic.hpp>
// 创建共享内存主题
shm_transport::Topic shm_topic(nh);
// 分配512KB共享内存缓冲区（优化用于1.5KB消息，约340帧缓冲）
shm_transport::Publisher shm_pub = shm_topic.advertise<sensor_msgs::LaserScan>("/scan", 1, 512 * 1024);
// 发布消息到共享内存
shm_pub.publish(scan_msg);
共享内存大小 = 单帧消息大小 × 缓冲帧数 共享内存大小由三个因素决定：
- 消息大小 ：通过分析ROS消息的序列化大小确定
- 缓冲需求 ：根据传感器频率和突发处理需求计算
- 资源平衡 ：预留足够缓冲应对突发，又不能过大浪费内存 当雷达消息约1.5KB，10Hz频率，为应对突发预留34秒缓冲（1.5KB × 340帧 = 510KB），所以分配512KB（1.5KB × 340帧）。 雷达单帧点数 = 扫描角度 ÷ 角度分辨率
从这个表格中，我们可以大概估算出此款雷达的单帧点数，
单帧点数 = 扫描角度 ÷ 角度分辨率 当频率是6HZ时候，角度分辨率是0.72，360➗0.72=500 点/帧
在 ydlidar.launch 文件中：
<param name="angle_min" value="0" />
<param name="angle_max" value="180" />
重新计算单帧消息大小
1. 采样点数的确定
// 采样率：3kHz，扫描频率：10Hz
// 每圈采样点数 = 采样率 / 扫描频率 = 3000 / 10 = 300点
// 180度扫描 = 300点 / 2 = 150点
2. 单帧消息大小计算
sensor_msgs::LaserScan {
// 元数据（约100 bytes）
std::string frame_id; // "laser_frame" ≈ 12 bytes
ros::Time stamp; // 8 bytes
float angle_min = 0; // 4 bytes
float angle_max = 180; // 4 bytes
float angle_increment; // 180度/150点 ≈ 1.2度/点
float scan_time; // 4 bytes
float time_increment; // 4 bytes
float range_min = 0.1; // 4 bytes
float range_max = 10.0; // 4 bytes
// 数据数组（关键！）
std::vector<float> ranges; // 150点 × 4bytes =
600 bytes
std::vector<float> intensities; // 150点 × 4bytes =
600 bytes
}
单帧消息大小 ≈ 100 + 600 + 600 = 1.3KB ≈ 1.5KB
重新计算缓冲容量
共享内存 = 512 KB = 524,288 bytes
缓冲帧数 = 524,288 / 1,536 ≈ 341 帧
缓冲时间 = 341帧 / 10Hz ≈ 34秒
关于看实际消息的大小，可以：
法一
# 查看主题消息大小
rostopic bw /scan # 查看带宽（bytes/s）
rostopic hz /scan # 查看频率
# 查看消息类型
rostopic info /scan
# 手动发布一条消息并查看大小
rostopic echo /scan | head -20
法2：在回调函数中添加打印
修改 shm_ydlidar_client.cpp 的 scanCallback 函数：
void scanCallback(const sensor_msgs::LaserScan::ConstPtr&
scan)
{
// ========== 新增：打印消息大小 ==========
int count = scan->scan_time / scan->time_increment;
// 计算消息总大小
size_t ranges_size = scan->ranges.size() * sizeof
(float);
size_t intensities_size = scan->intensities.size() *
sizeof(float);
size_t total_size = sizeof(*scan) + ranges_size +
intensities_size;
printf("[MSG SIZE]: 总消息大小 = %zu bytes\n",
total_size);
printf("[MSG SIZE]: ranges数组 = %zu bytes (%zu个点)\n",
ranges_size, scan->ranges.size());
printf("[MSG SIZE]: intensities数组 = %zu bytes (%zu个点)
\n", intensities_size, scan->intensities.size());
// =========================================
printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]
:\n", scan->header.frame_id.c_str(), count);
// ... 原有代码 ...
}
另外补充：
关于标准ROS订阅特点：（在ros1中）
-
- 消息通过TCP/IP传输
-
- 需要序列化/反序列化
-
- 数据需要从内核态拷贝到用户态
-
- 性能低于共享内存版本
SDK 获取硬件时间
// CYdLidar.cpp 第487-490行
uint64_t tim_scan_start = getTime(); // ← 获取硬件时间（纳
秒）
uint64_t startTs = tim_scan_start;
uint64_t tim_scan_end = getTime();
uint64_t endTs = tim_scan_end;
getTime() 的实现
// timer.h
#define getTime() impl::getCurrentTime()
// Linux 下实现（使用 gettimeofday）
uint64_t getCurrentTime() {
struct timeval tv;
gettimeofday(&tv, NULL); // ← 获取系统启动以来的时间
return (uint64_t)tv.tv_sec * 1000000000ull + tv.tv_usec
* 1000ull; // 转换为纳秒
}
注意 ：这里的"硬件时间"指的是 系统硬件时钟 ，不是雷达传感器内部时钟。 转换并赋值给 ROS 消息
// shm_ydlidar_node.cpp
ros::Time start_scan_time;
start_scan_time.sec = scan.stamp / 1000000000ul; // 纳秒 → 秒
start_scan_time.nsec = scan.stamp % 1000000000ul; // 剩余纳秒
scan_msg.header.stamp = start_scan_time; // 写入ROS消息时间戳
scan.stamp = 1,650,000,000,000 纳秒（1.65秒）//硬件时间戳
//这里的 scan.stamp 来自 SDK：
//uint64_t tim_scan_start = getTime(); // ← 获取硬件时间（纳秒）
sec = 1,650,000,000,000 / 1,000,000,000 = 1650 秒
nsec = 1,650,000,000,000 % 1,000,000,000 = 0 纳秒
scan_msg.header.stamp = ros::Time(1650, 0)
3. 硬件时间戳的作用 延迟计算的准确性
// shm_ydlidar_client.cpp 第22-26行
void scanCallback(const sensor_msgs::LaserScan::ConstPtr&
scan)
{
ros::Time publish_time = scan->header.stamp; // 硬件时
间戳（雷达扫描时刻）
ros::Time receive_time = ros::Time::now(); // 软件时
间戳（收到时刻）
ros::Duration delay = receive_time - publish_time; //
实际通信延迟
double delay_ms = delay.toSec() * 1000.0;
}
为什么这样计算更准确？
- publish_time = 雷达扫描开始的精确时间（SDK记录）
- receive_time = 订阅者收到数据的精确时间
- 两者差值 = 真实的端到端延迟
4. 软硬件时间戳对比
软件时间戳 ros::Time::now() 系统调用 硬件时间戳 scan.stamp SDK调用getTime() ``
