From ee3994248122b79b28cc02fd821ab195e8fde3e0 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 14 Aug 2024 13:51:06 -0700 Subject: [PATCH 1/4] Implementation that works mainly for LEGACY --- scripts/pcl_to_osf.py | 111 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 scripts/pcl_to_osf.py diff --git a/scripts/pcl_to_osf.py b/scripts/pcl_to_osf.py new file mode 100644 index 0000000..01841bf --- /dev/null +++ b/scripts/pcl_to_osf.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2024, Ouster, Inc. +# All rights reserved. + +""" +A module that shows how to subscribe to ouster point cloud and utililze pcl filters. +Author: Ussama Naal +""" + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, LivelinessPolicy +from sensor_msgs.msg import PointCloud2 +import point_cloud2 as pc2 +import pcl +from std_msgs.msg import String +from ouster.sdk.client import SensorInfo, ChanField, FieldType, FieldClass, LidarScan, destagger + +class PointCloudSubscriber(Node): + + def __init__(self): + super().__init__('point_cloud_subscriber') + + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + durability=DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + liveliness=LivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + depth=5 + ) + self.metadata_sub = self.create_subscription( + String, + '/ouster/metadata', + self.metadata_callback, + qos_profile=qos_profile) + + self._scan_ctr = 0 + + def metadata_callback(self, metadata): + import ouster.sdk.osf as osf + out_path = "sybmotic.osf" + self._sensor_info = SensorInfo(s=metadata.data) + metadata = SensorInfo(metadata.data) + self._writer = osf.Writer(out_path, self._sensor_info) + + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + durability=DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, + liveliness=LivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + depth=5 + ) + self.subscription = self.create_subscription( + PointCloud2, + '/ouster/points', + self.point_cloud_callback, + qos_profile=qos_profile) + + def point_cloud_callback(self, msg): + if self._scan_ctr > 1740: + return + + ts = msg.header.stamp + scan_ts = int(ts.sec * 1e9 + ts.nanosec) + scan = LidarScan(128, 1024) + + data = pc2.read_points(msg, field_names=["range"]) + points = np.array(list(data), dtype=np.uint32) + points = points.reshape((128, 1024)) + points = destagger(self._sensor_info, points, inverse=True) + scan.field(ChanField.RANGE)[:] = points + + data = pc2.read_points(msg, field_names=["reflectivity"]) + points = np.array(list(data), dtype=np.uint16) + points = points.reshape((128, 1024)) + points = destagger(self._sensor_info, points, inverse=True) + scan.field(ChanField.REFLECTIVITY)[:] = points + + data = pc2.read_points(msg, field_names=["ambient"]) + points = np.array(list(data), dtype=np.uint16) + points = points.reshape((128, 1024)) + points = destagger(self._sensor_info, points, inverse=True) + scan.field(ChanField.NEAR_IR)[:] = points + + data = pc2.read_points(msg, field_names=["intensity"]) + points = np.array(list(data), dtype=np.float32) + points = points.reshape((128, 1024)) + points = points.astype(np.uint32) + points = destagger(self._sensor_info, points, inverse=True) + scan.field(ChanField.SIGNAL)[:] = points + + self._writer.save(0, scan, scan_ts) + + self._scan_ctr += 1 + print("process scan", self._scan_ctr) + if self._scan_ctr > 1740: + self._writer.close() + print("done") + +def main(args=None): + rclpy.init(args=args) + point_cloud_subscriber = PointCloudSubscriber() + rclpy.spin(point_cloud_subscriber) + point_cloud_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 5c47c463d6b6a37a72c5a88b9a753cd21c4c6d6d Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 14 Aug 2024 13:56:55 -0700 Subject: [PATCH 2/4] Update module description --- scripts/pcl_to_osf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/pcl_to_osf.py b/scripts/pcl_to_osf.py index 01841bf..100924e 100644 --- a/scripts/pcl_to_osf.py +++ b/scripts/pcl_to_osf.py @@ -6,7 +6,7 @@ # All rights reserved. """ -A module that shows how to subscribe to ouster point cloud and utililze pcl filters. +A module that shows how to subscribe to ouster point cloud and save as OSF file. Author: Ussama Naal """ From 1e769d4a369f601ddb22d68798962378237c1921 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 15 Aug 2024 11:29:33 -0700 Subject: [PATCH 3/4] Read width/height from the message --- scripts/pcl_to_osf.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/scripts/pcl_to_osf.py b/scripts/pcl_to_osf.py index 100924e..ba4cf07 100644 --- a/scripts/pcl_to_osf.py +++ b/scripts/pcl_to_osf.py @@ -63,34 +63,38 @@ def point_cloud_callback(self, msg): return ts = msg.header.stamp - scan_ts = int(ts.sec * 1e9 + ts.nanosec) - scan = LidarScan(128, 1024) + scan_ts = np.uint64(ts.sec * 1e9 + ts.nanosec) + W, H = msg.width, msg.height + scan = LidarScan(H, W) data = pc2.read_points(msg, field_names=["range"]) points = np.array(list(data), dtype=np.uint32) - points = points.reshape((128, 1024)) + points = points.reshape((H, W)) points = destagger(self._sensor_info, points, inverse=True) scan.field(ChanField.RANGE)[:] = points data = pc2.read_points(msg, field_names=["reflectivity"]) points = np.array(list(data), dtype=np.uint16) - points = points.reshape((128, 1024)) + points = points.reshape((H, W)) points = destagger(self._sensor_info, points, inverse=True) scan.field(ChanField.REFLECTIVITY)[:] = points data = pc2.read_points(msg, field_names=["ambient"]) points = np.array(list(data), dtype=np.uint16) - points = points.reshape((128, 1024)) + points = points.reshape((H, W)) points = destagger(self._sensor_info, points, inverse=True) scan.field(ChanField.NEAR_IR)[:] = points data = pc2.read_points(msg, field_names=["intensity"]) points = np.array(list(data), dtype=np.float32) - points = points.reshape((128, 1024)) + points = points.reshape((H, W)) points = points.astype(np.uint32) points = destagger(self._sensor_info, points, inverse=True) scan.field(ChanField.SIGNAL)[:] = points + for i in range(len(scan.packet_timestamp)): + scan.packet_timestamp[i] = scan_ts + i * (1e9/100/16) + self._writer.save(0, scan, scan_ts) self._scan_ctr += 1 From cf6f1f82599ca876a222292c124aac95975f6f91 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 10 Oct 2024 10:00:28 -0700 Subject: [PATCH 4/4] Update output path --- scripts/pcl_to_osf.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/pcl_to_osf.py b/scripts/pcl_to_osf.py index ba4cf07..372ab38 100644 --- a/scripts/pcl_to_osf.py +++ b/scripts/pcl_to_osf.py @@ -22,7 +22,7 @@ class PointCloudSubscriber(Node): - def __init__(self): + def __init__(self, output_path): super().__init__('point_cloud_subscriber') qos_profile = QoSProfile( @@ -38,13 +38,13 @@ def __init__(self): qos_profile=qos_profile) self._scan_ctr = 0 + self._output_path = output_path def metadata_callback(self, metadata): import ouster.sdk.osf as osf - out_path = "sybmotic.osf" self._sensor_info = SensorInfo(s=metadata.data) metadata = SensorInfo(metadata.data) - self._writer = osf.Writer(out_path, self._sensor_info) + self._writer = osf.Writer(self._output_path, self._sensor_info) qos_profile = QoSProfile( reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, @@ -105,7 +105,7 @@ def point_cloud_callback(self, msg): def main(args=None): rclpy.init(args=args) - point_cloud_subscriber = PointCloudSubscriber() + point_cloud_subscriber = PointCloudSubscriber("output.osf") rclpy.spin(point_cloud_subscriber) point_cloud_subscriber.destroy_node() rclpy.shutdown()