rclcpp::init(argc, argv); //初始化ROS2节点
node_handle = rclcpp::Node::make_shared("rplidar_node"); //创建了一个名为"rplidar_node"的节点
//声明了一个名为"serial_port"的节点参数,
node_handle->declare_parameter("serial_port");//用于指定与RPLIDAR激光雷达通信的串口端口号
node_handle->declare_parameter("serial_baudrate");//指定通信串口的波特率
node_handle->declare_parameter("frame_id");//指定发布的激光雷达消息中的坐标系(frame ID)
node_handle->declare_parameter("inverted");//指定是否需要反转激光雷达的数据。
node_handle->declare_parameter("angle_compensate");//定是否需要对激光雷达的角度进行补偿
node_handle->declare_parameter("output_angle_min");//指定发布的激光雷达消息中的最小角度
node_handle->declare_parameter("output_angle_max");//指定发布的激光雷达消息中的最大角度
/*通过 "scan" 主题发布
sensor_msgs::msg::LaserScan 数据,
且发布队列的大小为 10。*/
scan_pub = node_handle->create_publisher<sensor_msgs::msg::LaserScan>("scan",10);
/*RPlidarDriver::CreateDriver() 方法是
RPLidar SDK 中用于创建驱动实例的方法。*/
drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
drv->startMotor();//启动雷达扫描
while(rclcpp::ok()) {
/*rplidar_response_measurement_node_hq_t nodes[360*8]
表示定义了一个数组,数组长度为 360*8=2880,
每个元素的类型是 rplidar_response_measurement_node_hq_t。
在 RPLidar 的 SDK 中,这个数组被用来存储从 RPLidar 设备读取到的激光扫描数据。
其中,rplidar_response_measurement_node_hq_t 是一个结构体,
它描述了一个激光点的属性:角度、距离、质量等。
每一次读取数据时,RPLidar 设备会发送多个激光点,
这些点的数据会被存储在 rplidar_response_measurement_node_hq_t 数组中,供程序进一步处理和分析。*/
rplidar_response_measurement_node_hq_t nodes[360*8];
/*从雷达获取一轮扫描的高质量数据,并将数据存储到 nodes 数组中。
如果获取数据失败,返回值为
RESULT_FAIL_BIT。*/
op_result = drv->grabScanDataHq(nodes, count);
if (op_result == RESULT_OK) {
/*将缓存区中的数据按角度从小到大排序,并将排序后的数据存储到 nodes 数组中*/
op_result = drv->ascendScanData(nodes, count);
if (op_result == RESULT_OK) {
if (angle_compensate) {//角度补偿
/*angle_compensate_nodes 数组中的每一个元素都是一个
rplidar_response_measurement_node_hq_t 类型的结构体,
包含了从雷达获取的一个测量值的信息,其中包括了距离和角度等信息。
具体来说,距离信息保存在结构体的 dist_mm_q2 字段中,单位为毫米。*/
publish_scan(angle_compensate_nodes, angle_compensate_nodes_count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id);
} else {
int start_node = 0, end_node = 0;
int i = 0;
// find the first valid node and last valid node
while (nodes[i++].dist_mm_q2 == 0);
start_node = i-1;
i = count -1;
while (nodes[i--].dist_mm_q2 == 0);
end_node = i+1;
angle_min = DEG2RAD(getAngle(nodes[start_node]));
angle_max = DEG2RAD(getAngle(nodes[end_node]));
publish_scan(&nodes[start_node], end_node-start_node +1,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id);
sdk/include/rplidar_cmd.h
typedef struct rplidar_response_measurement_node_hq_t {
_u16 angle_z_q14;
_u32 dist_mm_q2;
_u8 quality;
_u8 flag;
} __attribute__((packed)) rplidar_response_measurement_node_hq_t;