Extract lidar_packets from ROS2 bag file to dataframe in Python

[Grisly00]

When working with PointCloud2 data there exists a nice tool to import pointclouds from bag files to a dataset of Pandas dataframes. How could this work with a bag file of lidar_packets?
Topic in bag file:

/sensing/lidar/lidar_packets    │ ouster_msgs/msg/PacketMsg  

I played around with the ouster-sdk and rosbags, but could not figure out how to get from the high frequency stream of lidar_packets to single scans. Here is some code that leads me to

from pathlib import Path
from ouster import client
from ouster.client import LidarPacket
from rosbags.rosbag2 import Reader

BAG_FILE= "bag_fife_path"
METADATA_PATH= "ouster_metadata.txt"

def get_data(bag_file: Path, topic: str) -> list:
    data = []
    with Reader(bag_file) as reader:
        connections = [x for x in reader.connections if x.topic == topic]
        for connection, timestamp, rawdata in reader.messages(connections=connections):
            data.append(rawdata)
    return data


with open(METADATA_PATH, "r") as f:
    info = client.SensorInfo(f.read())

print(info)

packet_data = get_data(Path(BAG_FILE), "/sensing/lidar/lidar_packets")
packet = LidarPacket(packet_data[0], info=info, _raise_on_id_check=False)

if isinstance(packet, client.LidarPacket):
    # Now we can process the LidarPacket. In this case, we access
    # the measurement ids, timestamps, and ranges
    measurement_ids = packet.measurement_id
    timestamps = packet.timestamp
    ranges = packet.field(client.ChanField.RANGE)
    print(f"  encoder counts = {measurement_ids.shape}")
    print(f"  timestamps = {timestamps.shape}")
    print(f"  ranges = {ranges.shape}")
    print(packet)

Output:

encoder counts = (16,)
timestamps = (16,)
ranges = (64, 16)
<ouster.client.data.LidarPacket object at 0x7fcc5a07e950>

Does anyone have a tip on how to move forward from here?

Also, if I do not set the _raise_on_id_check flag to false I get the following error:
ouster.client.data.PacketIdError: Metadata init_id/sn does not match: expected by metadata - 9342166/992201000519, but got from packet buffer - 16641/26289897728

Platform (please complete the following information):

• Ouster Sensor? OS1
• Ouster Firmware Version? 2.3
• Programming Language? Python 3.10.2
• Operating System? Linux
• Machine Architecture? x64

[Samahu]

You could replay the bag file using the replay command of the ROS driver, during replay the driver publishes PointCloud2 messages on the /ouster/points topic which you can capture into a bag file using the ros2 bag record command and designate /ouster/points as the topic to be recorded. Then you could use the tool you have linked to process the bag file. Does this make sense?

[Grisly00]

@Samahu Thanks for your reply. This is exactly how we do it at the moment :)

There are though multiple problems with this. For one its an extra step that has to be done every time a file is recorded.
But mainly there are a few issues with rosbag2 that make it a bit unreliable (i.e. ros2/rosbag2#1256,
ros2/rosbag2#1276), Additionally, the Ouster driver has no QoS option to publish reliable which leads to some pointclouds getting lost in conversion.

In general playing and recording again adds another unnecessary step of complexity and potential way to loose some data.
That is why I would prefer to work with lidar_packets directly.

[Samahu]

@Samahu Thanks for your reply. This is exactly how we do it at the moment :)

There are though multiple problems with this. For one its an extra step that has to be done every time a file is recorded. But mainly there are a few issues with rosbag2 that make it a bit unreliable (i.e. ros2/rosbag2#1256, ros2/rosbag2#1276), Additionally, the Ouster driver has no QoS option to publish reliable which leads to some pointclouds getting lost in conversion.

In general playing and recording again adds another unnecessary step of complexity and potential way to loose some data. That is why I would prefer to work with lidar_packets directly.

I need to take a closer look at the two issues you linked but it is worth noting that I am adding the ability to utilize SystemDefaultsQoS (which is a reliable QoS policy) for all topics as part of the PRs: ouster-lidar/ouster-ros#146 and ouster-lidar/ouster-ros#149. Which is good because this would also applies to the PacketMsg(s).

[Samahu]

@Samahu Thanks for your reply. This is exactly how we do it at the moment :)
There are though multiple problems with this. For one its an extra step that has to be done every time a file is recorded. But mainly there are a few issues with rosbag2 that make it a bit unreliable (i.e. ros2/rosbag2#1256, ros2/rosbag2#1276), Additionally, the Ouster driver has no QoS option to publish reliable which leads to some pointclouds getting lost in conversion.
In general playing and recording again adds another unnecessary step of complexity and potential way to loose some data. That is why I would prefer to work with lidar_packets directly.

I need to take a closer look at the two issues you linked but it is worth noting that I am adding the ability to utilize SystemDefaultsQoS (which is a reliable QoS policy) for all topics as part of the PRs: ouster-lidar/ouster-ros#146 and ouster-lidar/ouster-ros#149. Which is good because this also applies to the PacketMsg(s).

[Grisly00]

Did not see this PR yet, looks like a very promising update!
Nevertheless, for many use cases it is much more efficient from a data volume and computation perspective to only save and work with raw packets instead of PointCloud2 messages.

[Samahu]

You will still have the ability to record the raw packets. So you can select the SystemDefaultsQoS policy (set use_system_default_qos to True) and record the raw packets; then replay the bag file containing the raw packets to produce the PointCloud2 messages. You could definitely read and process the raw packets directly from the bag file if you want but I could tell you right away that this is not something that we would be looking into in the near future.

[Grisly00]

You could definitely read and process the raw packets directly from the bag file if you want but I could tell you right away that this is not something that we would be looking into in the near future.

I understand that, but could you point me into the right direction considering my initial question? In theory all the functionality to convert raw packets to arrays of x,y,z should be available in the ouster-sdk, I reckon.

[Samahu]

You need to pass the LidarPacket to a ScanBatcher object from which you can obtain the LidarScan. Consider this piece of code: https://github.com/ouster-lidar/ouster_example/blob/ff46c961b690daae3e18e39999ef8651081cbc66/python/src/ouster/client/core.py#L418-L421

The ScanBatcher (batch variable) returns true when it has finished assembling the frame or the LidarScan.

[kairenw]

(Also, regarding your question re _raise_on_id_check it means that your metadata was not taken on the same init as your sensor, which is generally not recommended. If your data works fine, it's nothing to worry about but for the future I'd try to make sure new data is synced so you don't need to use the flag)

[Grisly00]

Thank you for your tips. Using ScanBatcher I got something out of the raw data, but I need to look into this in more detail. If I make some progress I will report it here.

[engnfrc]

Grisly00 I too am interested in doing exactly what you are doing. Have you made any progress with this? It would be great to have this provided directly from Ouster, I doubt we're the only ones interested in this.

[Grisly00]

@engnfrc I made some progress, but did not have any success yet. I created a repo with a minimal example script here including a test bag file. Feel free to check it out.

@Samahu Perhaps you can also take a look at my repo. I manage to create scans in the form of numpy arrays from a bag file containing lidar_packets, but they don't seem to be batched properly.