c++学习【22】雷达SDK发布数据到/scan

    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;