From 6a7693c551a34fc9f8dc47c8dda09cc64e2c57c0 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Fri, 25 Aug 2023 15:57:17 -0700 Subject: [PATCH] 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 --- CHANGELOG.rst | 4 +++ Dockerfile | 7 ++-- README.md | 4 +-- include/ouster_ros/os_ros.h | 15 ++++++++- launch/common.launch | 6 ++-- launch/driver.launch | 4 +++ launch/record.launch | 3 ++ launch/replay.launch | 3 ++ launch/sensor.launch | 4 +++ launch/sensor_mtp.launch | 4 +++ package.xml | 2 +- src/imu_packet_handler.h | 29 +++++++++++----- src/lidar_packet_handler.h | 59 ++++++++++++++++++++++----------- src/os_cloud_nodelet.cpp | 18 +++++----- src/os_driver_nodelet.cpp | 18 +++++----- src/os_ros.cpp | 15 ++------- src/os_sensor_nodelet_base.cpp | 4 +-- src/os_transforms_broadcaster.h | 5 ++- src/point_cloud_processor.h | 8 ++--- 19 files changed, 134 insertions(+), 78 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ecfb9fb4..21794558 100644 --- 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 index 34811a05..f7fed546 100644 --- a/Dockerfile +++ b/Dockerfile @@ -29,8 +29,8 @@ 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 \ @@ -38,9 +38,6 @@ RUN set -xe \ && 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 index 042dd3ef..9c97ffdc 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [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)

@@ -10,7 +10,7 @@ |:-----------:|:------:| | 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 index 8868a9c9..1eb70146 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -206,7 +206,20 @@ inline ouster::img_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(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 index 70c3bc9b..81af6c23 100644 --- 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."/> - + + + @@ -47,6 +48,7 @@ value="$(arg dynamic_transforms_broadcast_rate)"/> + diff --git a/launch/driver.launch b/launch/driver.launch index c3b4f633..7ecc5a82 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -25,6 +25,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + @@ -71,6 +73,8 @@ + diff --git a/launch/record.launch b/launch/record.launch index df445780..cfa39c3e 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -25,6 +25,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + @@ -89,6 +91,7 @@ + + @@ -64,6 +66,7 @@ + + + @@ -92,6 +95,7 @@ + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 39545c15..5f481009 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -29,6 +29,9 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + @@ -98,6 +101,7 @@ + diff --git a/package.xml b/package.xml index b4fea8ca..32ed19b4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.9.1 + 0.9.2 Ouster ROS driver ouster developers BSD diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h index ffd89113..b8349ba9 100644 --- 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; - // 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 \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index 9ee56005..7effc6ba 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -82,7 +82,6 @@ namespace sensor = ouster::sensor; using LidarScanProcessor = std::function; - class LidarPacketHandler { using LidarPacketAccumlator = std::function; @@ -93,7 +92,10 @@ class LidarPacketHandler { public: LidarPacketHandler(const ouster::sensor::sensor_info& info, - bool use_ros_time) { + const std::vector& 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(info); lidar_scan = std::make_unique( @@ -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& handlers) { - auto handler = std::make_shared(info, use_ros_time); - - handler->lidar_scan_handlers = handlers; - + const ouster::sensor::sensor_info& info, + const std::vector& handlers, + const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) { + auto handler = std::make_shared( + 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,7 +212,6 @@ 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, @@ -210,7 +219,6 @@ class LidarPacketHandler { curr_scan_first_nonzero_idx, curr_scan_first_nonzero_value, static_cast(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 \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index b6ea552d..75deb21f 100644 --- 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("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(ptp_utc_tai_offset * 1e+9)); imu_packet_sub = nh.subscribe( "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(ptp_utc_tai_offset * 1e+9)); lidar_packet_sub = nh.subscribe( "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) { lidar_packet_handler(msg->buf.data()); diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 65d15074..8c602cee 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -52,15 +52,16 @@ class OusterDriver : public OusterSensor { pnh.param("proc_mask", std::string{"IMU|IMG|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("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(ptp_utc_tai_offset * 1e+9)); } int num_returns = get_n_returns(info); @@ -102,12 +103,8 @@ class OusterDriver : public OusterSensor { } 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) { scan_pubs[i].publish(*msgs[i]); } @@ -151,7 +148,8 @@ class OusterDriver : public OusterSensor { if (check_token(tokens, "PCL") || check_token(tokens, "SCAN") || check_token(tokens, "IMG")) lidar_packet_handler = LidarPacketHandler::create_handler( - info, use_ros_time, processors); + info, processors, timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 10fe847a..63e88593 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -112,12 +112,6 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { } } -ros::Time ts_to_ros_time(uint64_t ts) { - ros::Time t; - t.fromNSec(ts); - return t; -} - } // namespace impl template & near_ir, const ouster::img_t& signal, const std::vector& pixel_shift_by_row) { auto timestamp = ls.timestamp(); - const auto rg = range.data(); const auto rf = reflectivity.data(); const auto nr = near_ir.data(); @@ -216,11 +209,9 @@ void copy_scan_to_cloud_destaggered( #endif for (auto u = 0; u < ls.h; u++) { for (auto v = 0; v < ls.w; v++) { - const auto ts_src_idx = (v + ls.w - pixel_shift_by_row[u]) % ls.w; - const auto col_ts = timestamp[ts_src_idx]; - const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL; - const auto src_idx = - u * ls.w + (v + ls.w - pixel_shift_by_row[u]) % ls.w; + const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w; + auto ts = timestamp[v_shift]; ts = ts > scan_ts ? ts - scan_ts : 0UL; + const auto src_idx = u * ls.w + v_shift; const auto tgt_idx = u * ls.w + v; const auto xyz = points.row(src_idx); cloud.points[tgt_idx] = ouster_ros::Point{ diff --git a/src/os_sensor_nodelet_base.cpp b/src/os_sensor_nodelet_base.cpp index b4681c62..6f4b1aec 100644 --- a/src/os_sensor_nodelet_base.cpp +++ b/src/os_sensor_nodelet_base.cpp @@ -6,13 +6,13 @@ * @brief implementation of OusterSensorNodeletBase interface */ -#include - #include "ouster_ros/os_sensor_nodelet_base.h" #include #include +#include + #include "ouster_ros/GetMetadata.h" namespace sensor = ouster::sensor; diff --git a/src/os_transforms_broadcaster.h b/src/os_transforms_broadcaster.h index 7213b553..ca6f12d2 100644 --- a/src/os_transforms_broadcaster.h +++ b/src/os_transforms_broadcaster.h @@ -89,6 +89,9 @@ class OusterTransformsBroadcaster { } const std::string& imu_frame_id() const { return imu_frame; } + const std::string& lidar_frame_id() const { return lidar_frame; } + const std::string& sensor_frame_id() const { return sensor_frame; } + const std::string& point_cloud_frame_id() const { return point_cloud_frame; } @@ -100,9 +103,9 @@ class OusterTransformsBroadcaster { std::string node_name; tf2_ros::StaticTransformBroadcaster static_tf_bcast; tf2_ros::TransformBroadcaster tf_bcast; // non-static - std::string sensor_frame; std::string imu_frame; std::string lidar_frame; + std::string sensor_frame; std::string point_cloud_frame; }; diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h index 7990c31a..f8e0fa0f 100644 --- a/src/point_cloud_processor.h +++ b/src/point_cloud_processor.h @@ -62,9 +62,9 @@ class PointCloudProcessor { void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const ros::Time& msg_ts) { for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { - scan_to_cloud_f_destaggered(cloud, - points, lut_direction, lut_offset, - scan_ts, lidar_scan, pixel_shift_by_row, i); + scan_to_cloud_f_destaggered(cloud, points, lut_direction, + lut_offset, scan_ts, lidar_scan, + pixel_shift_by_row, i); pcl_toROSMsg(cloud, *pc_msgs[i]); pc_msgs[i]->header.stamp = msg_ts; pc_msgs[i]->header.frame_id = frame; @@ -105,4 +105,4 @@ class PointCloudProcessor { PostProcessingFn post_processing_fn; }; -} // namespace ouster_ros \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file