Replicating VeloView's frame subdivision logic, resulting in numerous small frames

Question

I'm working on a custom app that decodes a data stream (PCAP or UDP packets) and I'm struggling to replicate VeloView's frame subdivision logic. The documentation for VelodynePlugin's interpreter is non-existent, so I decided to reverse engineering LidarView's code. In particular, I'm looking at vtkLidarReader::GetFrame and PacketConsumer::HandleSensorData, and it seems that different logic is used for reading from a PCAP file versus reading from a streaming source.

In file reading, all the packets contained in the file are read and processed by PreProcessPacketWrapped, which generates a vector of FrameInformation. When the end of file is reached, the function vtkLidarReader::GetFrame uses the FrameInformation in ProcessPacketWrapped to properly decode each frame.

Instead, when streaming, it directly calls ProcessPacketWrapped for each packet and adds the last decoded frame to the frame buffer when IsNewFrameAvailable is true.

However, I tried to replicate the streaming logic and I ended up with numerous small frames. After debugging, I suspect that certain packets with old firing data or non-progressive azimuth or value restarting from 0 are causing the interpreter to prematurely end the current frame.

I believe there must be additional framing subdivision logic that I'm missing. Can someone guide me on the correct functions to call in order to properly decode frames when reading from a stream of UDP packets?

Details:

Sensor -> HDL-32E

OS -> Ubuntu 22.04 x64

Version -> VeloView 5.1.0 Ubuntu 18.04-x86_64

PCAP test -> Drone_HDL32.pcap

Here there is a sample application to test the decoder (It needs libpcap, vtk, lidarview and velodynePlugin built or directly VeloView which contains all the dependencies. Build instructions).

customVelodynePacketInterpreter.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 */

customVelodynePacketInterpreter.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

The actual framing logic is within the packet interpreter, which decides if the current frame is finished (one packet after the other), and must calls SplitFrame if it's the case.

For Velodyne's Lidar, the call to SplitFrame happening here:

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

And the actual decision whether the new packet creates a new frame is within the FrameState keeper with hasChangedWithValue. The logic of hasChangedWithValue is to check whether the rotational increment changes signs. This meaning sill increasing is the same frame, still decreasing is the same frame, but switching from increasing to decreasing is a new frame.

That said, if you have erratic rotational values (due to device or network), it will create many small frames. If needed, you can have your own logic to discard this.