复制VeloView的帧细分逻辑，导致无数小帧

Question

我正在开发一个解码数据流（PCAP 或 UDP 数据包）的自定义应用程序，并且我正在努力复制 VeloView 的帧细分逻辑。 VelodynePlugin的解释器的文档不存在，所以我决定对LidarView的代码进行逆向工程。特别是，我正在查看

vtkLidarReader::GetFrame

和

PacketConsumer::HandleSensorData

，从 PCAP 文件读取与从流源读取似乎使用了不同的逻辑。

文件读取时，文件中包含的所有数据包都会被

PreProcessPacketWrapped

读取并处理，生成

FrameInformation

的向量。当到达文件末尾时，函数

vtkLidarReader::GetFrame

使用

FrameInformation

中的

ProcessPacketWrapped

来正确解码每一帧。

相反，在流式传输时，它直接为每个数据包调用

ProcessPacketWrapped

，并在

IsNewFrameAvailable

为 true 时将最后解码的帧添加到帧缓冲区。

但是，我尝试复制流逻辑，最终得到了许多小框架。 调试后，我怀疑某些带有旧发射数据或非渐进方位角或从 0 重新开始的值的数据包导致解释器过早结束当前帧。

我相信一定还有我缺少的额外框架细分逻辑。 有人可以指导我调用正确的函数，以便在读取 UDP 数据包流时正确解码帧吗？

详情：

传感器 -> HDL-32E

操作系统 -> Ubuntu 22.04 x64

版本 -> VeloView 5.1.0 Ubuntu 18.04-x86_64

PCAP 测试 -> Drone_HDL32.pcap

这里有一个测试解码器的示例应用程序（它需要构建libpcap，vtk，lidarview和velodynePlugin或直接VeloView，其中包含所有依赖项。构建说明）。

自定义VelodynePacketInterpreter.h

#ifndef CUSTOMVELODYNEPACKETINTERPRETER_H
#define CUSTOMVELODYNEPACKETINTERPRETER_H

#include "vtkVelodyneLegacyPacketInterpreter.h"

class customPacketInterpreter : public vtkVelodyneLegacyPacketInterpreter
{
    public:
        static customPacketInterpreter* New();
        void LoadFrameData(vtkSmartPointer<vtkPolyData> frame);

        vtkIdType GetNumberOfPoints(vtkSmartPointer<vtkPolyData> frame);
        double * GetPoint(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        unsigned char GetIntensity(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        unsigned char GetLaserId(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        unsigned short GetAzimuth(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        double GetDistanceM(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        unsigned long long GetAdjustedTime(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        unsigned int GetTimestamp(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);
        double GetVerticalAngle(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame);

    private:
        vtkPoints* frame_points;
        vtkAbstractArray* frame_intensity;
        vtkAbstractArray* frame_laserId;
        vtkAbstractArray* frame_azimuth;
        vtkAbstractArray* frame_distance;
        vtkAbstractArray* frame_adjustedTime;
        vtkAbstractArray* frame_timestamp;
        vtkAbstractArray* frame_verticalAngle;
};

#endif /* CUSTOMVELODYNEPACKETINTERPRETER_H */

自定义VelodynePacketInterpreter.cxx

#include "customVelodynePacketInterpreter.h"
#include "vtkPointData.h"

vtkStandardNewMacro(customPacketInterpreter)

void customPacketInterpreter::LoadFrameData(vtkSmartPointer<vtkPolyData> frame) {
    frame_points = frame->GetPoints();
    auto frame_data = frame->GetPointData();
    frame_intensity = frame_data->GetAbstractArray("intensity");
    frame_laserId = frame_data->GetAbstractArray("laser_id");
    frame_azimuth = frame_data->GetAbstractArray("azimuth");
    frame_distance = frame_data->GetAbstractArray("distance_m");
    frame_adjustedTime = frame_data->GetAbstractArray("adjustedtime");
    frame_timestamp = frame_data->GetAbstractArray("timestamp");
    frame_verticalAngle = frame_data->GetAbstractArray("vertical_angle");
}

vtkIdType customPacketInterpreter::GetNumberOfPoints(vtkSmartPointer<vtkPolyData> frame) {
    if (frame_points)
        return frame_points->GetNumberOfPoints();
    else
        return frame->GetPoints()->GetNumberOfPoints();
}

double * customPacketInterpreter::GetPoint(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_points)
        return frame_points->GetPoint(valueIdx);
    else
        return frame->GetPoints()->GetPoint(valueIdx);
}

unsigned char customPacketInterpreter::GetIntensity(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_intensity)
        return frame_intensity->GetVariantValue(valueIdx).ToUnsignedChar();
    else
        return frame->GetPointData()->GetArray("intensity")->GetVariantValue(valueIdx).ToUnsignedChar();
}

unsigned char customPacketInterpreter::GetLaserId(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_laserId)
        return frame_laserId->GetVariantValue(valueIdx).ToUnsignedChar();
    else
        return frame->GetPointData()->GetArray("laser_id")->GetVariantValue(valueIdx).ToUnsignedChar();
}

unsigned short customPacketInterpreter::GetAzimuth(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_azimuth)
        return frame_azimuth->GetVariantValue(valueIdx).ToUnsignedShort();
    else
        return frame->GetPointData()->GetArray("azimuth")->GetVariantValue(valueIdx).ToUnsignedShort();
}

double customPacketInterpreter::GetDistanceM(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_distance)
        return frame_distance->GetVariantValue(valueIdx).ToDouble();
    else
        return frame->GetPointData()->GetArray("distance_m")->GetVariantValue(valueIdx).ToDouble();
}

unsigned long long customPacketInterpreter::GetAdjustedTime(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_adjustedTime)
        return frame_adjustedTime->GetVariantValue(valueIdx).ToUnsignedLongLong();
    else
        return frame->GetPointData()->GetArray("adjustedtime")->GetVariantValue(valueIdx).ToUnsignedLongLong();
}

unsigned int customPacketInterpreter::GetTimestamp(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_timestamp)
        return frame_timestamp->GetVariantValue(valueIdx).ToUnsignedInt();
    else
        return frame->GetPointData()->GetArray("timestamp")->GetVariantValue(valueIdx).ToUnsignedInt();
}

double customPacketInterpreter::GetVerticalAngle(vtkIdType valueIdx, vtkSmartPointer<vtkPolyData> frame) {
    if (frame_verticalAngle)
        return frame_verticalAngle->GetVariantValue(valueIdx).ToDouble();
    else
        return frame->GetPointData()->GetArray("vertical_angle")->GetVariantValue(valueIdx).ToDouble();
}

main.cxx

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iomanip>
#include <fstream>
#include <sstream>
#include "pcap.h"
#include "customVelodynePacketInterpreter.h"

#define PCAP_SAVEFILE "./pcap_example/Drone_HDL32.pcap"
#define CALIB_FILE "HDL-32.xml"

typedef struct s_VelodynePointData {
    uint8_t laser_id;
    unsigned long long timestamp;
    double vertical_angle;
    double x;
    double y;
    double z;
    uint8_t intensity;
    uint16_t azimuth;
    double distance;
} VelodynePointData;

typedef std::vector<VelodynePointData> VelodyneFrame;

pcap_t *p;
unsigned long long point_id = 0;
int frame_id = 0;
std::ofstream outfile;
std::string filename;
std::vector<FrameInformation> frameCatalog;
std::vector<VelodyneFrame> frameBuffer;
VelodyneFrame buildingFrame;
customPacketInterpreter *interpreter;

int usage(char *progname)
{
    printf("Usage: [OPTIONS] %s [<savefile name>]\n", progname);
    printf("Options: \n");
    printf("\t-h\tShows this help message.\n");
    printf("\t-w\tWrite on csv files the decoded output.\n");
    exit(7);
}

void print_point(const VelodynePointData& data, std::ostream& out = std::cout) {
    out << point_id << ",";
    out << data.x << "," << data.y << "," << data.z << ",";
    out << data.timestamp << "," << data.azimuth << ",";
    out << data.distance << "," << static_cast<unsigned>(data.intensity) << ",";
    out << static_cast<unsigned>(data.laser_id) << "," << data.vertical_angle << std::endl;
}

VelodynePointData getVelodynePoint(unsigned long long point_id, vtkSmartPointer<vtkPolyData> frame) {
    double* point = interpreter->GetPoint(point_id, frame);
    uint8_t laser_id = interpreter->GetLaserId(point_id, frame);
    unsigned long long timestamp = interpreter->GetAdjustedTime(point_id, frame);
    double vertical_angle = interpreter->GetVerticalAngle(point_id, frame);
    uint8_t intensity = interpreter->GetIntensity(point_id, frame);
    uint16_t azimuth = interpreter->GetAzimuth(point_id, frame);
    double distance = interpreter->GetDistanceM(point_id, frame);

    VelodynePointData cloud_point {
        laser_id,
        timestamp,
        vertical_angle,
        point[0],
        point[1],
        point[2],
        intensity,
        azimuth,
        distance
    };

    return cloud_point;
}

double getElapsedTime(const timeval& now)
{
  struct timeval StartTime;
  StartTime.tv_sec = 0;
  StartTime.tv_usec = 0;
  return (now.tv_sec - StartTime.tv_sec) + (now.tv_usec - StartTime.tv_usec) / 1e6;
}

void openCsvFile() {
    std::stringstream ss_filename;
    ss_filename << "veloview_test_decoded/decoded (Frame " << frame_id << ").csv";
    filename = ss_filename.str();
    outfile = std::ofstream(filename, std::ofstream::out);
    if (outfile.is_open())
        outfile << "Point ID,X,Y,Z,adjustedtime,azimuth,distance_m,intensity,laser_id,vertical_angle" << std::endl;
    else {
        std::cout << "Error: Cannot open the file \"" << filename << "\" for writing." << std::endl;
        exit(1);
    }
    isNewFrame = false;

    std::cout << filename << std::endl;
}

void closeCurrentFrame() {
    point_id = 0;
    frame_id++;

    if (write_to_file && outfile.is_open())
        outfile.close();

    frameBuffer.push_back(buildingFrame);
    buildingFrame.clear();
}

// Called whenever an udp packet is read from the pcap file
void handle_packet(u_char *user, const struct pcap_pkthdr *hdr, const u_char *data) {
    if (!interpreter->IsValidPacket(&data[0x2A], 1206))
        return;

    interpreter->PreProcessPacketWrapped(&data[0x2A], 1206, fpos_t(), 0, &frameCatalog);

    timeval packetTime;
    gettimeofday(&packetTime, nullptr);
    interpreter->ProcessPacketWrapped(&data[0x2A], 1206, getElapsedTime(packetTime));

    // Check if there is a new complete frame
    if (interpreter->IsNewData()) {
        // Get the last available frame and fills the VelodynePoints vector
        vtkSmartPointer<vtkPolyData> lastFrame = interpreter->GetLastFrameAvailable();
        interpreter->LoadFrameData(lastFrame);
        auto numPoints = interpreter->GetNumberOfPoints(lastFrame);
        for (; point_id < numPoints; point_id++) {
            auto cloud_point = getVelodynePoint(point_id, lastFrame);
            buildingFrame.push_back(cloud_point);

            // Write the point on a csv file
            if (!outfile.is_open())
                openCsvFile();
            print_point(cloud_point, outfile);
        }
        closeCurrentFrame();

        // Clear the interpreter's internal frames buffer, leaving just the current building frame
        interpreter->ClearAllFramesAvailable();
    }
}

int main(int argc, char **argv)
{
    char filename[80];
    char errbuf[PCAP_ERRBUF_SIZE];
    char prestr[80];
    int majver = 0, minver = 0;

    interpreter = customPacketInterpreter::New();
    interpreter->LoadCalibration(CALIB_FILE);
    std::cout << std::setprecision(6) << std::fixed;

    strcpy(filename, PCAP_SAVEFILE);
    switch (argc)
    {
    case 2:
        if (strcmp(argv[1], "-w") == 0)
            write_to_file = true;
        else if (strcmp(argv[1], "-h") == 0)
            usage(argv[0]);
        else
            strcpy(filename,argv[1]);
        break;
    case 3:
        if (strcmp(argv[1], "-w") == 0)
            write_to_file = true;
        strcpy(filename,argv[2]);
        break;
    }
    if (argc > 3)
        usage(argv[0]);

    if (!(p = pcap_open_offline(filename, errbuf))) {
        fprintf(stderr,
            "Error in opening savefile, %s, for reading: %s\n",
            filename, errbuf);
        exit(2);
    }

    if (pcap_dispatch(p, 0, &handle_packet, (u_char *)0) < 0) {
        sprintf(prestr,"Error reading packets");
        pcap_perror(p,prestr);
        exit(4);
    }

    pcap_close(p);
}

Answer 1

实际的成帧逻辑位于数据包解释器内，它决定当前帧是否完成（一个数据包接一个数据包），如果是这种情况，则必须调用 SplitFrame。

对于 Velodyne 的激光雷达，对 SplitFrame 的调用发生在此处：

if (this->CurrentFrameState->hasChangedWithValue(*firingData))
{
  this->SplitFrame();
}

新数据包是否创建新帧的实际决定是在 FrameState keeper 中通过 hasChangedWithValue 进行的。 hasChangedWithValue的逻辑是检查旋转增量是否改变符号。 这意味着基台增加是同一帧，仍然减少是同一帧，但从增加切换到减少是一个新帧。

也就是说，如果您的旋转值不稳定（由于设备或网络），它将创建许多小框架。 如果需要，您可以有自己的逻辑来丢弃它。