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

* ROS-53: Add parameter for utc/tai offset in ptp mode. (#53) * Conditionally apply conversions to imu_gyro_ts for msg_ts. * Update changelog and package verison * Note which branch to be on to use galactic * Fix offset being negative and consider replay mode + formatting * Make sure all the timestamp values of LidarScan are utc corrected * Modify timestamp values before producing a PointCloud * Refactor the Dockerfile * Update Changelog and package version --------- Co-authored-by: Jack Geissinger <[email protected]>
ouster-lidar · Aug 25, 2023 · 6a7693c · 6a7693c
1 parent db68f4e
commit 6a7693c
Show file tree

Hide file tree

Showing 19 changed files with 134 additions and 78 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -7,6 +7,10 @@ Changelog
 ouster_ros(1)
 -------------
 * 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

diff --git a/Dockerfile b/Dockerfile
@@ -29,18 +29,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 package.xml $OUSTER_ROS_PATH/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 -y --from-paths $OUSTER_ROS_PATH
 
-# Set up build environment
-COPY --chown=build:build . $OUSTER_ROS_PATH
-
 USER build:build
 WORKDIR ${BUILD_HOME}
 

diff --git a/README.md b/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)

diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h
@@ -206,7 +206,20 @@ inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField field,
     return result;
 }
 
-ros::Time ts_to_ros_time(uint64_t ts);
+/**
+ * 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;
+}
+
+inline ros::Time ts_to_ros_time(uint64_t ts) {
+    ros::Time t;
+    t.fromNSec(ts);
+    return t;
+}
 
 }  // namespace impl
 

diff --git a/launch/common.launch b/launch/common.launch
@@ -17,8 +17,9 @@
     would set lidar_frame to be the frame used when publishing these messages."/>
 
   <arg name="timestamp_mode" doc="method used to timestamp measurements"/>
-  <arg name="dynamic_transforms_broadcast"
-    doc="static or dynamic transforms broadcast"/>
+  <arg name="ptp_utc_tai_offset" doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
+
+  <arg name="dynamic_transforms_broadcast" doc="static or dynamic transforms broadcast"/>
   <arg name="dynamic_transforms_broadcast_rate"
     doc="set the rate (Hz) of broadcast when using dynamic broadcast; minimum value is 1 Hz"/>
 
@@ -47,6 +48,7 @@
         value="$(arg dynamic_transforms_broadcast_rate)"/>
       <param name="~/proc_mask" value="$(arg proc_mask)"/>
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
+      <param name="~/ptp_utc_tai_offset" type="double" value="$(arg ptp_utc_tai_offset)"/>
     </node>
   </group>
 

diff --git a/launch/driver.launch b/launch/driver.launch
@@ -25,6 +25,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default=" " doc="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="true" doc="whether to run a rviz"/>
   <arg name="rviz_config" default="$(find ouster_ros)/config/viz.rviz" doc="optional rviz config file"/>
@@ -71,6 +73,8 @@
       <param name="~/udp_profile_lidar" type="str" value="$(arg udp_profile_lidar)"/>
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
+      <param name="~/ptp_utc_tai_offset" type="double"
+        value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
       <param name="~/tf_prefix" value="$(arg tf_prefix)"/>
       <param name="~/sensor_frame" value="$(arg sensor_frame)"/>

diff --git a/launch/record.launch b/launch/record.launch
@@ -25,6 +25,8 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="metadata" default="" doc="path to write metadata file when receiving sensor data"/>
   <arg name="bag_file" default="" doc="file name to use for the recorded bag file"/>
   <arg name="viz" default="true" doc="whether to run a rviz"/>
@@ -89,6 +91,7 @@
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
+    <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="dynamic_transforms_broadcast"
       value="$(arg dynamic_transforms_broadcast)"/>
     <arg name="dynamic_transforms_broadcast_rate"

diff --git a/launch/replay.launch b/launch/replay.launch
@@ -7,6 +7,8 @@
     possible values: {
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
   <arg name="viz" default="true" doc="whether to run a rviz"/>
   <arg name="rviz_config" default="$(find ouster_ros)/config/viz.rviz" doc="optional rviz config file"/>
 
@@ -64,6 +66,7 @@
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
+    <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="dynamic_transforms_broadcast"
       value="$(arg dynamic_transforms_broadcast)"/>
     <arg name="dynamic_transforms_broadcast_rate"

diff --git a/launch/sensor.launch b/launch/sensor.launch
@@ -25,6 +25,9 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
+
   <arg name="metadata" default=" " doc="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="true" doc="whether to run a rviz"/>
   <arg name="rviz_config" default="$(find ouster_ros)/config/viz.rviz" doc="optional rviz config file"/>
@@ -92,6 +95,7 @@
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
+    <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="_no_bond" value="$(arg _no_bond)"/>
     <arg name="dynamic_transforms_broadcast"
       value="$(arg dynamic_transforms_broadcast)"/>

diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
@@ -29,6 +29,9 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
+  <arg name="ptp_utc_tai_offset" default="-37.0"
+    doc="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
+
   <arg name="metadata" default=" " doc="path to write metadata file when receiving sensor data"/>
   <arg name="viz" default="true" doc="whether to run a rviz"/>
   <arg name="rviz_config" default="$(find ouster_ros)/config/viz.rviz" doc="optional rviz config file"/>
@@ -98,6 +101,7 @@
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
+    <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="_no_bond" value="$(arg _no_bond)"/>
     <arg name="dynamic_transforms_broadcast"
       value="$(arg dynamic_transforms_broadcast)"/>

diff --git a/package.xml b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.9.1</version>
+  <version>0.9.2</version>
   <description>Ouster ROS driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h
@@ -24,20 +24,31 @@ 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<ros::Time(const uint8_t*)>;
-        // clang-format off
-        auto timestamper = use_ros_time ?
-            Timestamper{[](const uint8_t* /*imu_buf*/) {
-                return ros::Time::now(); }} :
-            Timestamper{[pf](const uint8_t* imu_buf) {
-                return impl::ts_to_ros_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 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 impl::ts_to_ros_time(ts);
+                }};
+        } else {
+            timestamper = Timestamper{[pf](const uint8_t* imu_buf) {
+                return impl::ts_to_ros_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);
         };
     }
 };
 
-}   // namespace ouster_ros
+}  // namespace ouster_ros
diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h
@@ -82,7 +82,6 @@ namespace sensor = ouster::sensor;
 using LidarScanProcessor =
     std::function<void(const ouster::LidarScan&, uint64_t, const ros::Time&)>;
 
-
 class LidarPacketHandler {
     using LidarPacketAccumlator = std::function<bool(const uint8_t*)>;
 
@@ -93,7 +92,10 @@ class LidarPacketHandler {
 
    public:
     LidarPacketHandler(const ouster::sensor::sensor_info& info,
-                       bool use_ros_time) {
+                       const std::vector<LidarScanProcessor>& handlers,
+                       const std::string& timestamp_mode,
+                       int64_t ptp_utc_tai_offset)
+        : lidar_scan_handlers{handlers} {
         // initialize lidar_scan processor and buffer
         scan_batcher = std::make_unique<ouster::ScanBatcher>(info);
         lidar_scan = std::make_unique<ouster::LidarScan>(
@@ -107,14 +109,23 @@ class LidarPacketHandler {
         };
         const sensor::packet_format& pf = sensor::get_format(info);
 
-        lidar_packet_accumlator =
-            use_ros_time
-                ? LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
-                      return lidar_handler_ros_time(pf, lidar_buf);
-                  }}
-                : LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
-                      return lidar_handler_sensor_time(pf, lidar_buf);
-                  }};
+        if (timestamp_mode == "TIME_FROM_ROS_TIME") {
+            lidar_packet_accumlator =
+                LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
+                    return lidar_handler_ros_time(pf, lidar_buf);
+                }};
+        } else if (timestamp_mode == "TIME_FROM_PTP_1588") {
+            lidar_packet_accumlator = LidarPacketAccumlator{
+                [this, pf, ptp_utc_tai_offset](const uint8_t* lidar_buf) {
+                    return lidar_handler_sensor_time_ptp(pf, lidar_buf,
+                                                         ptp_utc_tai_offset);
+                }};
+        } else {
+            lidar_packet_accumlator =
+                LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
+                    return lidar_handler_sensor_time(pf, lidar_buf);
+                }};
+        }
     }
 
     LidarPacketHandler(const LidarPacketHandler&) = delete;
@@ -129,12 +140,11 @@ class LidarPacketHandler {
 
    public:
     static HandlerType create_handler(
-        const ouster::sensor::sensor_info& info, bool use_ros_time,
-        const std::vector<LidarScanProcessor>& handlers) {
-        auto handler = std::make_shared<LidarPacketHandler>(info, use_ros_time);
-
-        handler->lidar_scan_handlers = handlers;
-
+        const ouster::sensor::sensor_info& info,
+        const std::vector<LidarScanProcessor>& handlers,
+        const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) {
+        auto handler = std::make_shared<LidarPacketHandler>(
+            info, handlers, timestamp_mode, ptp_utc_tai_offset);
         return [handler](const uint8_t* lidar_buf) {
             if (handler->lidar_packet_accumlator(lidar_buf)) {
                 for (auto h : handler->lidar_scan_handlers) {
@@ -202,15 +212,13 @@ class LidarPacketHandler {
         assert(idx != ts_v.data() + ts_v.size());  // should never happen
         int curr_scan_first_nonzero_idx = idx - ts_v.data();
         uint64_t curr_scan_first_nonzero_value = *idx;
-
         uint64_t scan_ns = curr_scan_first_nonzero_idx == 0
                                ? curr_scan_first_nonzero_value
                                : impute_value(last_scan_last_nonzero_idx,
                                               last_scan_last_nonzero_value,
                                               curr_scan_first_nonzero_idx,
                                               curr_scan_first_nonzero_value,
                                               static_cast<int>(ts_v.size()));
-
         last_scan_last_nonzero_idx =
             find_if_reverse(ts_v, [](uint64_t h) { return h != 0; });
         assert(last_scan_last_nonzero_idx >= 0);  // should never happen
@@ -242,6 +250,19 @@ class LidarPacketHandler {
         return true;
     }
 
+    bool lidar_handler_sensor_time_ptp(const sensor::packet_format&,
+                                       const uint8_t* lidar_buf,
+                                       int64_t ptp_utc_tai_offset) {
+        if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false;
+        auto ts_v = lidar_scan->timestamp();
+        for (int i = 0; i < ts_v.rows(); ++i)
+            ts_v[i] = impl::ts_safe_offset_add(ts_v[i], ptp_utc_tai_offset);
+        lidar_scan_estimated_ts = compute_scan_ts(ts_v);
+        lidar_scan_estimated_msg_ts =
+            impl::ts_to_ros_time(lidar_scan_estimated_ts);
+        return true;
+    }
+
     bool lidar_handler_ros_time(const sensor::packet_format& pf,
                                 const uint8_t* lidar_buf) {
         auto packet_receive_time = ros::Time::now();
@@ -289,4 +310,4 @@ class LidarPacketHandler {
     LidarPacketAccumlator lidar_packet_accumlator;
 };
 
-}   // namespace ouster_ros
+}  // namespace ouster_ros
diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp
@@ -93,15 +93,16 @@ class OusterCloud : public nodelet::Nodelet {
         auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
         auto tokens = parse_tokens(proc_mask, '|');
 
-        auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{});
-        bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME";
+        auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
+        double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
 
         auto& nh = getNodeHandle();
 
         if (check_token(tokens, "IMU")) {
             imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 100);
             imu_packet_handler = ImuPacketHandler::create_handler(
-                info, tf_bcast.imu_frame_id(), use_ros_time);
+                info, tf_bcast.imu_frame_id(), timestamp_mode,
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
             imu_packet_sub = nh.subscribe<PacketMsg>(
                 "imu_packets", 100, [this](const PacketMsg::ConstPtr msg) {
                     auto imu_msg = imu_packet_handler(msg->buf.data());
@@ -152,12 +153,8 @@ class OusterCloud : public nodelet::Nodelet {
             }
 
             processors.push_back(LaserScanProcessor::create(
-                info,
-                tf_bcast
-                    .point_cloud_frame_id(),  // TODO: should we allow having a
-                                              // different frame for the laser
-                                              // scan than point cloud???
-                scan_ring, [this](LaserScanProcessor::OutputType msgs) {
+                info, tf_bcast.lidar_frame_id(), scan_ring,
+                [this](LaserScanProcessor::OutputType msgs) {
                     for (size_t i = 0; i < msgs.size(); ++i) {
                         if (msgs[i]->header.stamp > last_msg_ts)
                             last_msg_ts = msgs[i]->header.stamp;
@@ -168,7 +165,8 @@ class OusterCloud : public nodelet::Nodelet {
 
         if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) {
             lidar_packet_handler = LidarPacketHandler::create_handler(
-                info, use_ros_time, processors);
+                info, processors, timestamp_mode,
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
             lidar_packet_sub = nh.subscribe<PacketMsg>(
                 "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
                     lidar_packet_handler(msg->buf.data());