Reading and Writing Scans to and fro from An OSF or PCAP file (for non-ROS users)

[hirenpatel1207]

The documentation has no examples on how to store lidar scans in PCAP file. Also it has no documentation on how to write to a OSFfile.

Describe the solution you'd like
For non-ROS applications an example on how to efficiently write lidar data to storage (in PCAP format) and then read it .

Describe alternatives you've considered
The application is Windows only so using ROS driver is not feasible. Also the Ouster Studio has option to store PCAP files, but that is not open source.
Maybe a example can be formed using that project.

Targeted Platform (please complete the following information only if applicable, otherwise dot N/A):

• Ouster Sensor: OS-1
• Ouster Firmware Version? [feature only applicable to specific fw version]
• Programming Language C++
• Operating System: Windows
• Machine Architecture x86

[diego-guridi]

Hi hirenpatel1207

You can try doing something like this for recording in pcap:

#include <iostream>
#include <fstream>

#include "ouster/client.h"
#include "ouster/os_pcap.h"


void WriteToFile(const std::string& filename, const std::string& data) {
    std::ofstream file(filename);
    file << data;
    file.close();
}

int main() {
    const std::string sensor_hostname = "<sensor hostname>";
    const std::string pcap_file_name = "my_pcap_file.pcap";
    const std::string metadata_file_name = "my_pcap_file.json";

    auto client_handle = ouster::sensor::init_client(sensor_hostname, 7502, 7503);
    if (!client_handle) {
        std::cerr << "Failed to connect" << std::endl;
    }

    std::cout << "Gathering metadata..." << std::endl;
    auto metadata = ouster::sensor::get_metadata(*client_handle, 10);
    std::cout << "  done." << std::endl;

    ouster::sensor::sensor_info sensor_info(metadata);
    ouster::sensor::packet_format packet_format = ouster::sensor::get_format(sensor_info);

    auto lidar_packet = ouster::sensor::LidarPacket(packet_format.lidar_packet_size);
    auto imu_packet = ouster::sensor::ImuPacket(packet_format.imu_packet_size);

    std::cout << "Recording ... " << std::endl; 
    WriteToFile(metadata_file_name, sensor_info.to_json_string());
    auto record_handle = ouster::sensor_utils::record_initialize(pcap_file_name, 1500);
    while(true) {
        ouster::sensor::client_state st = ouster::sensor::poll_client(*client_handle);

        if (st & ouster::sensor::LIDAR_DATA) {
            if (!ouster::sensor::read_lidar_packet(*client_handle, lidar_packet)) {
                std::cerr << "failed to read lidar packet" << std::endl;
                break;
            }

            ouster::sensor_utils::record_packet( 
                *record_handle,
                "192.168.0.1",
                "192.168.0.1",
                7502, 7502,
                lidar_packet.buf.data(), lidar_packet.buf.size(),
                lidar_packet.host_timestamp);
        }
        if (st & ouster::sensor::IMU_DATA) {
            if (!ouster::sensor::read_imu_packet(*client_handle, imu_packet)) {
                std::cerr << "failed to read imu packet" << std::endl;
                break;
            } 

            ouster::sensor_utils::record_packet(
                *record_handle,
                "192.168.0.1",
                "192.168.0.1",
                7503, 7503,
                imu_packet.buf.data(), imu_packet.buf.size(),
                imu_packet.host_timestamp);
        }
    }
    ouster::sensor_utils::record_uninitialize(*record_handle);
}

And then, for reading:

#include "ouster/os_pcap.h"
#include "ouster/lidar_scan.h"


int main() {
    const std::string pcap_file = "my_pcap_file.pcap";
    const std::string json_file = "my_pcap_file.json";

    auto metadata = ouster::sensor::metadata_from_json(json_file);
    ouster::sensor::Packet packet(packet_format.lidar_packet_size);
    ouster::sensor_utils::packet_info packet_info;

    auto width = metadata.format.columns_per_frame;
    auto height = metadata.format.pixels_per_column;
    ouster::LidarScan lidar_scan = ouster::LidarScan(width, height, metadata.format.udp_profile_lidar);
    ouster::ScanBatcher scan_batcher(metadata.format.columns_per_frame, packet_format);

    auto pcap_handle = ouster::sensor_utils::replay_initialize(pcap_file);

     while (ouster::sensor_utils::next_packet_info(*pcap_handle, packet_info)) {
        auto packet_size = ouster::sensor_utils::read_packet(*pcap_handle, packet.buf.data(), packet.buf.size());

        if (packet_size == packet_format.lidar_packet_size) {
            const auto &lidar_packet = static_cast<ouster::sensor::LidarPacket &>(packet);
            if (scan_batcher(lidar_packet, lidar_scan)) {
                // do something with lidar scan here
            }
        }
     }
}

I hope that helps

[hirenpatel1207]

Thanks @diego-guridi for the example, I was able to record some PCAP files.
To avoid the blocking while loop. I am trying to use QTimers. If I move the stuff within the while loop to QTimer callback, then the method ouster::sensor::poll_client(*_sensorClientHandle) returns value "6". What does that mean? there is nothing defined for value "6" in enum client_state.

[matt-attack]

The return value for poll_client is a bitmask rather than an enum, so 6 would mean there is both IMU and Lidar data available (4 + 2).

[hirenpatel1207]

Hi, The above code samples work well.
But now I want to convert Lidar_scan to pcl type (like PointXYZIR). I tried to follow ouster-ros code to do that. But that is heavly templated. Is there anywhere else where I can have a look?

[diego-guridi]

Hi @hirenpatel1207, you can check out the representations_example.cpp that shows how you can get a cartesian point cloud from a LidarScan and how to work with "fields" from the LidarScan to get data like reflectivity.

With that you should be able to create PCL type objects or directly write PCD files.

I hope this helps!

[Samahu]

@hirenpatel1207 checkout the example in the ouster-ros-extras repo https://github.com/ouster-lidar/ouster-ros-extras/blob/ros2/scripts/ros2_range_2_points_xyz.py this isn't directly what you are asking but you can easily combine the python examples to obtain the range field from LidarScan then apply that geenrates a PointCloud object from the ouster-ros-extras

[Samahu]

Here is a complete example:

from ouster.sdk import open_source
from ouster.sdk import client
import pcl

scan_source = open_source(source_url)
xyzlut = client.XYZLut(scan_source.metadata)

for idx, scan in enumerate(scan_source):
        range = scan.field(ChanField.RANGE)
        cv_image2 = client.destagger(scan_source.metadata, range)
        xyz = self.xyzlut(cv_image2)
        cloud = pcl.PointCloud()
        cloud.from_array(xyz..astype(np.float32), 'float32')
        # Save the point cloud
        pcl.save(cloud, f"cloud_{idx:3.pcd")

Something around these lines!

Hope this helps!

[hirenpatel1207]

Thank you all for pointing to the right sources.
I must use C++ because my aim is to conver the points to a pcl::PointCloud object, to use the pcl library further.
I was able to get atleast the xyz and reflectivity from the LidarScan.
But I am not sure about other fields especially the point timestamp.

In the ouster-ros, the timestamp field is nano-secs since the scan start.
Can some one verify if the following code is correct, especially, that the point timestamp is correctly calculated?

LidarScan lidar_scan;
sensor::sensor_info metadata;

// read the meta data and also first lidar scan from the pcap file

// Create a pcl::PointCloud from the lidar_scan
pcl::PointCloud<OusterPointXYZIRT>::Ptr cloud;               //OusterPointXYZIRT definition taken from ouster-ros sdk
cloud.reset(new pcl::PointCloud<OusterPointXYZIRT>());
cloud->resize(lidar_scan.h * lidar_scan.w);


Eigen::Array<uint32_t, -1, -1, Eigen::RowMajor> reflectivity;
if (metadata.format.udp_profile_lidar ==
    sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) {
    reflectivity = lidar_scan.field<uint8_t>(sensor::ChanField::REFLECTIVITY)
                       .cast<uint32_t>();
} else if (metadata.format.udp_profile_lidar ==
           sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) {
    reflectivity = lidar_scan.field<uint8_t>(sensor::ChanField::REFLECTIVITY)
                       .cast<uint32_t>();
} else {  // legacy or single return profile
    reflectivity = lidar_scan.field<uint16_t>(sensor::ChanField::REFLECTIVITY)
                       .cast<uint32_t>();
}

std::vector<int> pixel_shift_by_row = metadata.format.pixel_shift_by_row;
auto timestamp = lidar_scan.timestamp();

auto range = lidar_scan.field(sensor::ChanField::RANGE);
LidarScan::Points scanCloud = cartesian(range, ouster::make_xyz_lut(metadata));

for (size_t row = 0; row < lidar_scan.h; row++)
   {
   for (size_t column = 0; column < lidar_scan.w; column++)
      {
      const auto v_shift = (column + lidar_scan.w - pixel_shift_by_row[row]) % lidar_scan.w;
      auto ts = timestamp[v_shift];                                                   // is the timestamp correctly calculated?
      uint pointIndex = row * lidar_scan.w + v_shift;
      OusterPointXYZIRT pt;
      auto xyz = scanCloud.row(pointIndex);
      pt.x = xyz(0);
      pt.y = xyz(1);
      pt.z = xyz(2);
      pt.t = static_cast<uint32_t>(ts);                    // Is this absolute timestamp or nano-secs since scan start time? How to get the scan start time?
      pt.ring = static_cast<uint16_t>(row);
      pt.reflectivity = reflectivity(row, column);
      // how to get the range?
      cloud->push_back(pt);
      }
   }

[Samahu]

pt.t = static_cast<uint32_t>(ts); // Is this absolute timestamp or nano-secs since scan start time? How to get the scan start time?

this is the absolute time, you need to do ts[x] - ts[0] to get the relative time, however note that this is tricky since ts[0] isn't always valid due to azimuth window settings or if you have the first packet dropped.

another thing to note is that you don't want to use v_shift here but rather ts = timestamp[v]; to have these timestamps properly aligned with the points.