HesaiLidar_SDK_2.0-2.0.10版本获取单帧点运数据重复3倍

[coldbath]

雷达: Pandar_XT
SDK版本:2.0.10

问题描述:
在SDK的 void hesai_driver::Run() 函数中解析帧数据, 保存的点云数据发现数据重复.请问该如何解决?

CODE:
void hesai_driver::Run()
{
LogInfo("--------begin to parse udp package--------");
is_thread_runing_ = true;
UdpFrame_t udp_packet_frame;
uint32_t packet_index = 0;
// uint32_t start = GetMicroTickCount();
UdpPacket packet;
FaultMessageInfo fault_message_info;

    uint32_t total_packet_count = 0;
    uint32_t total_packet_loss_count = 0;
    while (is_thread_runing_)
    {

    //get one packte from origin_packets_buffer_, which receive data from upd or pcap thread
    int ret = lidar_ptr_->GetOnePacket(packet);
    if (ret == -1) continue;
    //get fault message
    if (packet.packet_len == kFaultMessageLength 
        || (packet.buffer[0] == 0xCD && packet.buffer[1] == 0xDC)
        || (packet.buffer[SOMEIP_OFFSET] == 0xCD && packet.buffer[SOMEIP_OFFSET + 1] == 0xDC)) {
        if(device_fault_port_ != 0){
        if(device_ip_address_ != packet.ip || device_fault_port_ != packet.port){
            continue;
        }
        }
        ret = lidar_ptr_->ParserFaultMessage(packet, fault_message_info);
        if (ret == 0) {
        if (fault_message_cb_)
            fault_message_cb_(fault_message_info);
        }
        continue;
    }

    // Wait for initialization to complete before starting to parse the point cloud
    if(!lidar_ptr_->GetInitFinish(AllInitFinish)) continue;

    if(device_udp_port_ != 0 && (packet.is_timeout == false || packet_index == 0)){
        if(device_ip_address_ != packet.ip || device_udp_port_ != packet.port) {
        continue;
        }
    }

    //get distance azimuth reflection, etc.and put them into decode_packet
    ret = lidar_ptr_->DecodePacket(lidar_ptr_->frame_, packet);
    if (lidar_ptr_->frame_.imu_config.flag) {
        if (imu_cb_ && !last_imu_data_.isSameImuValue(lidar_ptr_->frame_.imu_config)) {
        imu_cb_(lidar_ptr_->frame_.imu_config);
        last_imu_data_ = lidar_ptr_->frame_.imu_config;
        }
    }
    if(ret != 0) {
        continue;
    }
    

    //one frame is receive completely, split frame
    if(lidar_ptr_->frame_.scan_complete) {
        if (lidar_ptr_->frame_.fParam.use_cuda) {
#ifdef USE_CUDA
        if (lidar_ptr_->frame_.packet_num > 0) {
            ret = gpu_parser_ptr_.ComputeXYZI(lidar_ptr_->frame_);  
        }
        else {
            ret = -1;
            LogError("packet_num is 0, cuda compute xyz error");
        }
#endif
        } else {
            //waiting for parser thread compute xyzi of points in the same frame
            while(!lidar_ptr_->ComputeXYZIComplete(packet_index)) std::this_thread::sleep_for(std::chrono::microseconds(100));
        }
        if (lidar_ptr_->frame_.fParam.remake_config.flag) {
        auto& rq = lidar_ptr_->frame_.fParam.remake_config;
        lidar_ptr_->frame_.points_num = rq.max_azi_scan * rq.max_elev_scan;
        }
        if (lidar_ptr_->frame_.points_num == 0) {
        uint32_t points_num = 0;
        for (uint32_t i = 0; i < lidar_ptr_->frame_.packet_num; i++) {
            if (points_num != i * lidar_ptr_->frame_.per_points_num) {
            memcpy(lidar_ptr_->frame_.points + points_num, lidar_ptr_->frame_.points + 
                    i * lidar_ptr_->frame_.per_points_num, lidar_ptr_->frame_.getPointSize() * lidar_ptr_->frame_.valid_points[i]);
            }
            points_num += lidar_ptr_->frame_.valid_points[i];
        }
        lidar_ptr_->frame_.points_num = points_num;
        }
        //log info, display frame message
        if (ret == 0 && lidar_ptr_->frame_.points_num > kMinPointsOfOneFrame) {
              

            //publish upd packet topic
            if(pkt_cb_) {
                pkt_cb_(udp_packet_frame, lidar_ptr_->frame_.frame_start_timestamp);
            }

            if (pkt_loss_cb_ )
            {
                total_packet_count = lidar_ptr_->GetGeneralParser()->seqnum_loss_message_.total_packet_count;
                total_packet_loss_count = lidar_ptr_->GetGeneralParser()->seqnum_loss_message_.total_loss_count;
                pkt_loss_cb_(total_packet_count, total_packet_loss_count);
            }
            if (ptp_cb_ && lidar_ptr_->frame_.frame_index % 100 == 1)
            {
                u8Array_t ptp_status;
                u8Array_t ptp_lock_offset;
                int ret_status = lidar_ptr_->ptc_client_->GetPTPDiagnostics(ptp_status, 1); // ptp_query_type = 1
                int ret_offset = lidar_ptr_->ptc_client_->GetPTPLockOffset(ptp_lock_offset);
                if (ret_status != 0 || ret_offset != 0)
                {
                    LogInfo("-->%d %d %zu %zu", ret_status, ret_offset, ptp_status.size(), ptp_lock_offset.size());
                }
                else
                {
                    ptp_cb_(ptp_lock_offset.front(), ptp_status);
                }
                }
                if (correction_cb_ && lidar_ptr_->frame_.frame_index % 1000 == 1)
                {
                    correction_cb_(lidar_ptr_->correction_string_);
                }

                uint32_t points_nums = lidar_ptr_->frame_.points_num;
                
                LidarPointXYZICRTT*  lidar_raw_data = reinterpret_cast<LidarPointXYZICRTT *>(lidar_ptr_->frame_.points);

                hesaiLidarFrame cur_frame;
                cur_frame.timestamp = lidar_ptr_->frame_.points[0].timeSecond * 1000000000ULL + lidar_ptr_->frame_.points[0].timeNanosecond;

                while (points_nums)
                {
                            Vec3_t pt_hesai(lidar_raw_data->x, lidar_raw_data->y, lidar_raw_data->z);
                            Vec3_t pt_livox = R_lidar_hesai * pt_hesai + t_lidar_hesai;

                            PointXYZIRT cur_pt_;

                            cur_pt_.x          = pt_livox.x();
                            cur_pt_.y          = pt_livox.y();
                            cur_pt_.z          = pt_livox.z();
                            cur_pt_.intensity  = lidar_raw_data->intensity;
                            cur_pt_.timestamp  = lidar_raw_data->timeSecond * 1000000000ULL + lidar_raw_data->timeNanosecond;
                            cur_pt_.ring       = lidar_raw_data->ring;
                            if(cur_pt_.x != 0.0 || cur_pt_.y != 0.0 || cur_pt_.z != 0.0)
                                cur_frame.points.push_back(cur_pt_);

                            ++lidar_raw_data;
                            points_nums--;
                }

                // uint64_t sysTime = getCurTimeInt1();
                // std::cout << "cur_frame pt size = " << cur_frame.points.size() << " time = " << cur_frame.timestamp << "  sysTime = " << sysTime << std::endl;
                PushDataToCache(cur_frame);

                if(bCalib && cur_frame.points.size() != 0)
                {
                    calib_cache_.push(cur_frame);
                    bCalib = false;
                }
            
        } 
        //reset frame variable
        lidar_ptr_->frame_.Update();
        if (lidar_ptr_->frame_.fParam.update_function_safety_flag) {
        lidar_ptr_->frame_.clearFuncSafety();
        }
        if (lidar_ptr_->frame_.fParam.remake_config.flag) {
        memset(lidar_ptr_->frame_.points, 0, lidar_ptr_->frame_.getPointSize() * lidar_ptr_->frame_.maxPackerPerFrame * lidar_ptr_->frame_.maxPointPerPacket);
        }
        //clear udp packet vector
        udp_packet_frame.clear();
        packet_index = 0;

        //if the packet which contains split frame msgs is valid, it will be the first packet of new frame
        if(packet.is_timeout == false) {
        lidar_ptr_->DecodePacket(lidar_ptr_->frame_, packet);
        if (lidar_ptr_->frame_.fParam.use_cuda == false) {
            lidar_ptr_->ComputeXYZI(packet_index);
        }
        udp_packet_frame.emplace_back(packet);
        packet_index++;
        }
    }
    else {
        //new decoded packet of one frame, put it into decoded_packets_buffer_ and compute xyzi of points
        if(lidar_ptr_->frame_.packet_num != packet_index) {
        if (lidar_ptr_->frame_.fParam.use_cuda == false) {
            lidar_ptr_->ComputeXYZI(packet_index);
        }
        udp_packet_frame.emplace_back(packet);
        packet_index++;
        } 

        //update status manually if start a new frame failedly
        if (packet_index >= lidar_ptr_->frame_.maxPackerPerFrame) {
        LogError("fail to start a new frame, packet_index: %d", packet_index);
        packet_index = 0;
        udp_packet_frame.clear();
        lidar_ptr_->ClearPacketBuffer();
        std::this_thread::sleep_for(std::chrono::microseconds(100));
        lidar_ptr_->frame_.Update();
        lidar_ptr_->GetUdpParser()->SetComputePacketNumToZero();
        }
    }
    }
} 

[changxingshuo]

能否检查一下points_nums的值，是否符合预期，以及cur_frame的size是否符合预期。
如果points_nums符合预期，但cur_frame超出，那么问题在你的while循环里。
如果lidar_ptr_->frame_.points_num已经超出，那么parser部分的代码是否存在修改？