Using Python SDK, Lidar packets seem to stop at frame id 65535

[kpanyam]

I am trying to read Lidar and IMU packets using the Python SDK 0.5.1 from a OS-1-32-U0 sensor running 2.4.0 firmware. When the frame_id reaches 65535, I no longer seem to receive any more Lidar packets, but the IMU packets do keep coming. Am I doing something wrong? My code looks something like this:

with closing(Sensor(hostname)) as source:
    for packet in source:
         if isinstance(packet, ImuPacket):
               ....
               continue
         if isinstance(packet, LidarPacket):

[kairenw]

That seems bad! Mind sharing what the entire code looks like? You can also submit it through our customer support if you don't want to share the code

[kpanyam]

Copyright (c) 2021, Ati Motors 
All rights reserved.
"""

from absl import logging
from absl import flags
from absl import app
from contextlib import closing
import numpy as np
from ouster import client
# from ouster.client import *
import time
from typing import cast, Dict, Iterable, Iterator, List, Optional, Tuple, Union

from ati.common.bus import Bus
from ati.schema import LidarFrame, LidarLookupTable, ImuUpdate, Vector
from ati.common.config import load_mule_config

flags.DEFINE_boolean("debug", False, "debug")
flags.DEFINE_string("sensor", "192.168.1.100", "Sensor IP address")

def send_imu_data(sock, accel, gyro): 
    imu = ImuUpdate ( 
                <SNIP>
            )
    sock.send("mule.sensor.imu", imu);

def send_lidar_data(sock, W, H, frame_id, data, metadata, debug=False): 
    if metadata is not None:
        lookup = LidarLookupTable(
                     <SNIP>
                 )
        sock.send("mule.sensor.ouster_lookup_table", lookup)

    frame = LidarFrame(
                <SNIP>
            )
    sock.send("mule.sensor.lidar", frame)

MAXTIMEOUT = 1000

def read_packets(hostname, W: int = 1024, H: int = 32, h_offset: float = 0, debug: bool = False) -> None:
    sock = Bus()
    with closing(Sensor(hostname)) as source:
        fname = "/data/metadata.json"
        source.write_metadata(fname)
        with open(fname, 'r') as f:
            metadata_bytes = f.read()

        metadata = source.metadata
        xyzlut = client.XYZLut(metadata)

        nimu = 0
        nlidar = 0
        ntimeout = 0
        frame_id = -1
        nsectors = 0

        def new_frame():
            return np.zeros((H, W), dtype=np.int64)

        def process_packet(ranges, packet):
            m_ids = packet.measurement_id
            packet_ranges = packet.field(client.ChanField.RANGE)
            N = len(m_ids)
            for i in m_ids:
                ranges[:, i] = packet_ranges[:, i%N]

        def process_frame(ranges):
            """returns a point cloud in the shape (32768, 9).. currently ignoring intensity, etc."""
            xyz = xyzlut(ranges).reshape(-1, 3) 
            frame =  np.zeros((H * W, 9), dtype=np.float32)
            frame[:, :3] = client.destagger(metadata, xyz).reshape(-1, 3)
            frame[:, 3] = np.tile(np.array(metadata.beam_altitude_angles) * np.pi / 180, W) # vertical angles (rad)
            ranges = client.destagger(metadata,  ranges).reshape(-1)
            frame[:, 4] = (ranges*0.001).reshape(-1)
            frame[:, 5] = np.repeat(np.linspace(0, np.pi*2, W), H) # horizontal angles
            return frame

        while True:
            try:
                for packet in source:
                    if isinstance(packet, ImuPacket):
                        send_imu_data(sock, packet.accel, packet.angular_vel)
                        nimu += 1
                        if debug and nimu % 1000 == 0:
                            logging.info(f"imu {nimu} {packet.angular_vel}")
                        continue

                    if isinstance(packet, LidarPacket):
                        if debug and nlidar % 100 == 0:
                            logging.info(f"{packet.frame_id}")

                        if packet.frame_id > 65530:
                            logging.info(f"got packet {packet.frame_id} {packet.measurement_id[0]}")

                        if packet.frame_id > frame_id:
                            if frame_id > 0:
                                frame = process_frame(ranges)
                                lookup = metadata_bytes if nlidar < 5 else None
                                send_lidar_data(sock, W, H, frame_id, frame, lookup, debug)

                            # new frame
                            frame_id = packet.frame_id
                            ranges = new_frame()
                            nsectors = 0
                            nlidar += 1

                        process_packet(ranges, packet)
                        nsectors += 1
                        continue

                    logging.warning(f"unknown packet type {type(packet)}")
            except Exception as e:
                logging.warning(f"Exception {e}", exc_info=True)
                ntimeout += 1
                if ntimeout > MAXTIMEOUT:
                    logging.fatal(f"Too many exceptions, quit")

def main(argv) -> None:
    sensor = "192.168.1.100"
    if flags.FLAGS.sensor:
         sensor = flags.FLAGS.sensor

    debug = False
    if flags.FLAGS.debug:
         debug = True

    config = load_mule_config()
    W = config.get("drivers.ouster.horizontal_angles", 1024)
    H = config.get("drivers.ouster.vertical_beams", 32)
    horizontal_offset = config.get("drivers.ouster.horizontal_offset", 0)
    read_packets(sensor, W, H, horizontal_offset, debug)

if __name__ == "__main__":
    app.run(main)

[kpanyam]

I think I understand what's happening, when I look at longer logs. The frame id is getting reset to 0 after 65536 in the python SDK, which broke my code. This is definitely not the case in the sample C code, which we currently use to build our driver. Is it possible to fix this 16-bit limitation in the SDK? Is it intentional, or is it a bug?

[kairenw]

Hi @kpanyam,

So packet.frame_id is parsed straight from the packet header, where you can see it is defined to be 16 bits.

In the C++ code when we do the frame batching, we actually use this logic here where we essentially use a != comparison instead of a > comparison and handle the possibility of out of order packets first.

In the other ticket you opened, my colleague indicated how you might create a class that does something similar to the code here without having to handle the batching logic yourself since _client.ScanBatcher basically calls the C++ logic that I linked up there.

Please let me know if you have any other questions!

[kpanyam]

Thanks for the explanation. I didn't realise that the packet frame_id is 16 bits only.
I have rewritten our code to use != to compare the frame_id, and we will maintain our own frame id which is monotonic.

[kairenw]

Awesome! Glad that got worked out. I'm going to close out the ticket now. Let me know if there's any other questions