rplidar_ros代码解析之角度补偿

if (angle_compensate) {
                    const int angle_compensate_nodes_count = 360;
                    const int angle_compensate_multiple = 1;
                    int angle_compensate_offset = 0;
                    rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
                    memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
                    int i = 0, j = 0;
                    for( ; i < count; i++ ) {
                        if (nodes[i].distance_q2 != 0) {
                            float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
                            int angle_value = (int)(angle * angle_compensate_multiple);
                            if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
                            for (j = 0; j < angle_compensate_multiple; j++) {
                                angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
                            }
                        }
                    }
  
                    publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
                             start_scan_time, scan_duration, inverted,
                             angle_min, angle_max,
                             frame_id);
                }

         rplidar_ros工程-node.cpp-main()中，做角度补偿的初衷是：ROS订阅的scan话题消息中，角度值按照固定角间隔递增排列。而rplidar处理后的角度值却是不固定角间隔。因此，角度补偿的目的是，调整角度表示以符合scan话题消息的格式。

         举个实例，rplidar是按照顺时针旋转的，角度大小递增，顺时针为负。假设rplidar旋转一圈，得到了如下的扫描点，角度递增排序后分别是-25.9，-16.3，-5.7。

i=0, angle=-25.9, angle_value =-26, angle_compensate_offset=-26,故angle_compensate_nodes[0]=nodes[0];

i=1, angle=-16.3, angle_value=-16, angle_compensate_offset=-26, 故angle_compensate_nodes[10]=nodes[1];

i=2, angle=-5.7, angle_value=-6, angle_compensate_offset=-26, 故angle_compensate_nodes[20]=nodes[2];

         经过以上步骤，rplidar的扫描点角度全部转换为正值，且按照角度值大小递增的顺序填充入扫描点集合，该集合的范围是[0,360]度，步长是1度。这样就可以向scan话题发布消息了。