SW-5459: add a parameter for utc/tai time offset (#198)

* Implement automatic UTC/TAI adjustment for the PTP timestamp mode
* Destagger timestamps when generating desaggered point clouds

CHANGELOG.rst

@@ -5,6 +5,10 @@ Changelog
 [unreleased]
 ============
 * breaking: publish PCL point clouds destaggered.
+* introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds
+  to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode
+  is used.
+* fix: destagger columns timestamp when generating destaggered point clouds.
 
 
 ouster_ros v0.10.0

Dockerfile

@@ -3,7 +3,7 @@ ARG ROS_DISTRO=rolling
 FROM ros:${ROS_DISTRO}-ros-core AS build-env
 ENV DEBIAN_FRONTEND=noninteractive \
     BUILD_HOME=/var/lib/build \
-    OUSTER_ROS_PATH=/opt/catkin_ws/src/ouster-ros
+    OUSTER_ROS_PATH=/opt/ros2_ws/src/ouster-ros
 
 RUN set -xue \
 # Turn off installing extra packages globally to slim down rosdep install
@@ -28,21 +28,15 @@ RUN set -xe \
 && groupadd -o -g ${BUILD_GID} build \
 && useradd -o -u ${BUILD_UID} -d ${BUILD_HOME} -rm -s /bin/bash -g build build
 
-# Install build dependencies using rosdep
-COPY --chown=build:build \
-    ouster-ros/package.xml \
-    $OUSTER_ROS_PATH/ouster-ros/package.xml
+# Set up build environment
+COPY --chown=build:build . $OUSTER_ROS_PATH
 
 RUN set -xe         \
 && apt-get update   \
 && rosdep init      \
 && rosdep update --rosdistro=$ROS_DISTRO \
-&& rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src \
-&& apt remove -y ros-${ROS_DISTRO}-ouster-msgs
-
+&& rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src
 
-# Set up build environment
-COPY --chown=build:build . $OUSTER_ROS_PATH
 
 USER build:build
 WORKDIR ${BUILD_HOME}

LICENSE

@@ -4,7 +4,7 @@ Upstream-Contact: Ouster Sensor SDK Developers <[email protected]>
 Source: https://github.com/ouster-lidar/ouster_example
 
 Files: *
-Copyright: 2018-2022 Ouster, Inc
+Copyright: 2018-2023 Ouster, Inc
 License: BSD-3-Clause
 
 Files: include/optional-lite/*

README.md

@@ -2,15 +2,15 @@
 
 [ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) |
 [ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
-[ROS2 (foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
+[ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
 
 <p style="float: right;"><img width="20%" src="docs/images/logo.png" /></p>
 
 | ROS Version | Build Status (Linux) |
 |:-----------:|:------:|
 | ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
 | ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
-| ROS2 (foxy) | [![foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
+| ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
 
 - [Overview](#overview)
 - [Requirements](#requirements)
@@ -49,7 +49,7 @@ setup ROS on your machine before proceeding with the remainder of this guide.
 > If you have _rosdep_ tool installed on your system you can then use the following command to get all
     required dependencies:  
     ```
-    rosdep install -y --from-paths $OUSTER_ROS_PATH -r
+    rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src
     ```
 
 > **Note**

ouster-ros/config/driver_params.yaml

@@ -26,6 +26,9 @@ ouster/os_driver:
     #                       packet of a LidarScan as the timestamp of the IMU,
     #                       PointCloud2 and LaserScan messages.
     timestamp_mode: ''
+    # ptp_utc_tai_offset[optional]: UTC/TAI offset in seconds to apply when
+    # TIME_FROM_PTP_1588 timestamp mode is used.
+    ptp_utc_tai_offset: -37.0
     # udp_profile_lidar[optional]: lidar packet profile; possible values:
     # - LEGACY: not recommended
     # - RNG19_RFL8_SIG16_NIR16_DUAL

ouster-ros/config/os_sensor_cloud_image_params.yaml

@@ -22,6 +22,7 @@ ouster/os_cloud:
     imu_frame: os_imu
     point_cloud_frame: os_lidar
     timestamp_mode: ''  # this value needs to match os_sensor/timestamp_mode
+    ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588
     proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options
     use_system_default_qos: False # needs to match the value defined for os_sensor node
     scan_ring: 0  # Use this parameter in conjunction with the SCAN flag and choose a

ouster-ros/include/ouster_ros/os_ros.h

@@ -211,6 +211,17 @@ inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField field,
     ouster::impl::visit_field(ls, field, read_and_cast(), result);
     return result;
 }
+
+/**
+ * simple utility function that ensures we don't wrap around uint64_t due
+ * to a negative value being bigger than ts value in absolute terms.
+ * @remark method does not check upper boundary
+ */
+inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) {
+    return offset < 0 && ts < static_cast<uint64_t>(std::abs(offset)) ? 0 : ts + offset;
+}
+
+
 } // namespace impl
 
 }  // namespace ouster_ros

ouster-ros/launch/record.independent.launch.xml

@@ -33,6 +33,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=""
     description="path to write metadata file when receiving sensor data"/>
   <arg name="bag_file" default=""
@@ -86,6 +88,7 @@
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
+      <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>

ouster-ros/launch/replay.independent.launch.xml

@@ -10,6 +10,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+   <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=""
     description="path to write metadata file when receiving sensor data"/>
   <arg name="bag_file"
@@ -55,6 +57,7 @@
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
+      <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>

ouster-ros/launch/sensor.composite.launch.xml

@@ -33,6 +33,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=""
     description="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="true"
@@ -78,6 +80,7 @@
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
+      <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>

ouster-ros/launch/sensor.independent.launch.xml

@@ -33,6 +33,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=""
     description="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="true"
@@ -84,6 +86,7 @@
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
+      <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>

ouster-ros/launch/sensor_mtp.launch.xml

@@ -41,6 +41,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+   <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=""
     description="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="true"
@@ -86,6 +88,7 @@
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
+      <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>

ouster-ros/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.10.1</version>
+  <version>0.10.2</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

ouster-ros/src/image_processor.h

@@ -149,7 +149,7 @@ class ImageProcessor {
             for (size_t v = 0; v < W; v++) {
                 const size_t vv = (v + W - px_offset[u]) % W;
                 const size_t idx = u * W + vv;
-                 // TODO: re-examine this truncation later
+                // TODO: re-examine this truncation later
                 // 16 bit img: use 4mm resolution and throw out returns > 260m
                 auto r = (rg[idx] + 0b10) >> 2;
                 range_image_map(u, v) = r > pixel_value_max ? 0 : r;

ouster-ros/src/imu_packet_handler.h

@@ -8,7 +8,7 @@
 
 #pragma once
 
-// prevent clang-format from altering the location of "ouster_ros/ros.h", the
+// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the
 // header file needs to be the first include due to PCL_NO_PRECOMPILE flag
 // clang-format off
 #include "ouster_ros/os_ros.h"
@@ -24,21 +24,33 @@ class ImuPacketHandler {
    public:
     static HandlerType create_handler(const ouster::sensor::sensor_info& info,
                                       const std::string& frame,
-                                      bool use_ros_time) {
+                                      const std::string& timestamp_mode,
+                                      int64_t ptp_utc_tai_offset) {
         const auto& pf = ouster::sensor::get_format(info);
         using Timestamper = std::function<rclcpp::Time(const uint8_t*)>;
-        // clang-format off
-        auto timestamper = use_ros_time ?
-            Timestamper{[](const uint8_t* /*imu_buf*/) {
-                return rclcpp::Clock(RCL_ROS_TIME).now(); }} :
-            Timestamper{[pf](const uint8_t* imu_buf) {
-                return rclcpp::Time(pf.imu_gyro_ts(imu_buf)); }};
-        // clang-format on
+        Timestamper timestamper;
+
+        if (timestamp_mode == "TIME_FROM_ROS_TIME") {
+            timestamper = Timestamper{[](const uint8_t* /*imu_buf*/) {
+                return rclcpp::Clock(RCL_ROS_TIME).now();
+            }};
+        } else if (timestamp_mode == "TIME_FROM_PTP_1588") {
+            timestamper =
+                Timestamper{[pf, ptp_utc_tai_offset](const uint8_t* imu_buf) {
+                    uint64_t ts = pf.imu_gyro_ts(imu_buf);
+                    ts = impl::ts_safe_offset_add(ts, ptp_utc_tai_offset);
+                    return rclcpp::Time(ts);
+                }};
+        } else {
+            timestamper = Timestamper{[pf](const uint8_t* imu_buf) {
+                return rclcpp::Time(pf.imu_gyro_ts(imu_buf));
+            }};
+        }
+
         return [&pf, &frame, timestamper](const uint8_t* imu_buf) {
-            return packet_to_imu_msg(pf, timestamper(imu_buf),
-                                                 frame, imu_buf);
+            return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf);
         };
     }
 };
 
-}   // namespace ouster_ros
+}  // namespace ouster_ros

ouster-ros/src/laser_scan_processor.h

@@ -14,7 +14,6 @@
 #include "ouster_ros/os_ros.h"
 // clang-format on
 
-
 namespace ouster_ros {
 
 class LaserScanProcessor {
@@ -66,4 +65,4 @@ class LaserScanProcessor {
     PostProcessingFn post_processing_fn;
 };
 
-}   // namespace ouster_ros
+}  // namespace ouster_ros