From dd08f80bbffa35895526abbe515b0e8508986b45 Mon Sep 17 00:00:00 2001 From: Jack Geissinger Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 01/10] ROS-53: Add parameter for utc/tai offset in ptp mode. (#53) * ptp: Add parameter for utc/tai offset in ptp mode. * Conditionally apply conversions to imu_gyro_ts for msg_ts. --- include/ouster_ros/os_ros.h | 6 ++++ launch/common.launch | 6 ++-- launch/driver.launch | 5 +++ launch/record.launch | 3 ++ launch/sensor.launch | 5 +++ src/imu_packet_handler.h | 25 ++++++++++---- src/lidar_packet_handler.h | 59 +++++++++++++++++++++------------ src/os_cloud_nodelet.cpp | 15 ++++----- src/os_driver_nodelet.cpp | 15 ++++----- src/os_ros.cpp | 6 ++++ src/os_transforms_broadcaster.h | 5 ++- 11 files changed, 103 insertions(+), 47 deletions(-) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 8868a9c9..06e24103 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -206,6 +206,12 @@ inline ouster::img_t get_or_fill_zero(sensor::ChanField field, return result; } +/** + * simple utility to function that ensures we don't wrap around uint64_t due + * to negative value bigger than ts value + */ +uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset); + ros::Time ts_to_ros_time(uint64_t ts); } // 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..51725191 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -25,6 +25,9 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + @@ -71,6 +74,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 @@ + + + @@ -92,6 +95,8 @@ + diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h index ffd89113..329e96ec 100644 --- a/src/imu_packet_handler.h +++ b/src/imu_packet_handler.h @@ -24,15 +24,28 @@ 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; + Timestamper timestamper; // 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)); }}; + 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)); + }}; + } + // clang-format on return [&pf, &frame, timestamper](const uint8_t* imu_buf) { return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf); diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index 9ee56005..a3b905df 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -93,7 +93,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 +110,22 @@ 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,12 @@ 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 +213,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 +220,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 @@ -227,9 +236,7 @@ class LidarPacketHandler { const uint8_t* lidar_buf, const ros::Time current_time) { auto curr_scan_first_arrived_idx = packet_col_index(pf, lidar_buf); - auto delta_time = ros::Duration( - 0, - std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); + auto delta_time = ros::Duration(0, std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); return current_time - delta_time; } @@ -237,8 +244,16 @@ class LidarPacketHandler { const uint8_t* lidar_buf) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); - lidar_scan_estimated_msg_ts = - impl::ts_to_ros_time(lidar_scan_estimated_ts); + lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); + 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; + lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); + lidar_scan_estimated_ts = impl::ts_safe_offset_add(lidar_scan_estimated_ts, ptp_utc_tai_offset); + lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); return true; } diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index b6ea552d..d7542e4a 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()); @@ -153,10 +154,7 @@ 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??? + 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) @@ -168,7 +166,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..4f7453f1 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); @@ -103,10 +104,7 @@ 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??? + 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 +149,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..1429b1b2 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -112,6 +112,12 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { } } +uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) { + if (offset >= 0 || ts > static_cast(std::abs(offset))) + ts += offset; + return ts; +} + ros::Time ts_to_ros_time(uint64_t ts) { ros::Time t; t.fromNSec(ts); 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; }; From 1df06549533896440df842c5a02b24e1ef7a2a52 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 02/10] Update changelog and package verison --- CHANGELOG.rst | 3 +++ launch/sensor.launch | 3 +-- launch/sensor_mtp.launch | 4 ++++ package.xml | 2 +- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ecfb9fb4..88d29958 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,6 +7,9 @@ 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. ouster_ros v0.10.0 diff --git a/launch/sensor.launch b/launch/sensor.launch index 9d6ca5af..942d23af 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -95,8 +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 From d9fa8c925fd7076cec4ff65f89e36d50475afc58 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 03/10] Note which branch to be on to use galactic --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 042dd3ef..9ce9107e 100644 --- a/README.md +++ b/README.md @@ -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) From 40573751adfff1fa50d919ba44498bc71af1cf3b Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 04/10] Add missing tag --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9ce9107e..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)

From 3bbfb9f689241e066865c8c151805c4d96142a2b Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 05/10] Fix offset being negative and consider replay mode + formatting --- include/ouster_ros/os_ros.h | 14 +++++++++++--- launch/driver.launch | 1 - launch/replay.launch | 3 +++ src/imu_packet_handler.h | 20 +++++++++----------- src/lidar_packet_handler.h | 33 +++++++++++++++++++-------------- src/os_cloud_nodelet.cpp | 5 ++--- src/os_driver_nodelet.cpp | 5 ++--- src/os_ros.cpp | 12 ------------ src/os_sensor_nodelet_base.cpp | 4 ++-- src/point_cloud_processor.h | 8 ++++---- 10 files changed, 52 insertions(+), 53 deletions(-) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 06e24103..1c2396a6 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -208,11 +208,19 @@ inline ouster::img_t get_or_fill_zero(sensor::ChanField field, /** * simple utility to function that ensures we don't wrap around uint64_t due - * to negative value bigger than ts value + * to a negative value being bigger than ts value in absolute terms. + * @remark method does not check upper boundary */ -uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset); +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; +} + -ros::Time ts_to_ros_time(uint64_t ts); +inline ros::Time ts_to_ros_time(uint64_t ts) { + ros::Time t; + t.fromNSec(ts); + return t; +} } // namespace impl diff --git a/launch/driver.launch b/launch/driver.launch index 51725191..7ecc5a82 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -27,7 +27,6 @@ }"/> - diff --git a/launch/replay.launch b/launch/replay.launch index 7f2080fb..97d58e9e 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -7,6 +7,8 @@ possible values: { TIME_FROM_ROS_TIME }"/> + @@ -64,6 +66,7 @@ + ; Timestamper timestamper; - // clang-format off if (timestamp_mode == "TIME_FROM_ROS_TIME") { - timestamper = Timestamper{[](const uint8_t* /*imu_buf*/) { - return ros::Time::now(); - }}; + 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); - }}; + 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)); }}; } - // clang-format on 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 a3b905df..c2e27826 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; @@ -95,8 +94,8 @@ class LidarPacketHandler { LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::vector& handlers, const std::string& timestamp_mode, - int64_t ptp_utc_tai_offset) : - lidar_scan_handlers{handlers} { + 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( @@ -116,9 +115,10 @@ class LidarPacketHandler { 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); + 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 = @@ -142,8 +142,7 @@ class LidarPacketHandler { static HandlerType create_handler( const ouster::sensor::sensor_info& info, const std::vector& handlers, - const std::string& timestamp_mode, - int64_t ptp_utc_tai_offset) { + 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) { @@ -236,7 +235,9 @@ class LidarPacketHandler { const uint8_t* lidar_buf, const ros::Time current_time) { auto curr_scan_first_arrived_idx = packet_col_index(pf, lidar_buf); - auto delta_time = ros::Duration(0, std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); + auto delta_time = ros::Duration( + 0, + std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); return current_time - delta_time; } @@ -244,16 +245,20 @@ class LidarPacketHandler { const uint8_t* lidar_buf) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); - lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); + lidar_scan_estimated_msg_ts = + impl::ts_to_ros_time(lidar_scan_estimated_ts); return true; } bool lidar_handler_sensor_time_ptp(const sensor::packet_format&, - const uint8_t* lidar_buf, int64_t ptp_utc_tai_offset) { + const uint8_t* lidar_buf, + int64_t ptp_utc_tai_offset) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); - lidar_scan_estimated_ts = impl::ts_safe_offset_add(lidar_scan_estimated_ts, ptp_utc_tai_offset); - lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); + lidar_scan_estimated_ts = impl::ts_safe_offset_add( + lidar_scan_estimated_ts, ptp_utc_tai_offset); + lidar_scan_estimated_msg_ts = + impl::ts_to_ros_time(lidar_scan_estimated_ts); return true; } @@ -304,4 +309,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 d7542e4a..75deb21f 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -153,9 +153,8 @@ class OusterCloud : public nodelet::Nodelet { } processors.push_back(LaserScanProcessor::create( - info, - tf_bcast.lidar_frame_id(), - 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; diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 4f7453f1..8c602cee 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -103,9 +103,8 @@ class OusterDriver : public OusterSensor { } processors.push_back(LaserScanProcessor::create( - info, - tf_bcast.lidar_frame_id(), - 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]); } diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 1429b1b2..cf93461a 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -112,18 +112,6 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { } } -uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) { - if (offset >= 0 || ts > static_cast(std::abs(offset))) - ts += offset; - return ts; -} - -ros::Time ts_to_ros_time(uint64_t ts) { - ros::Time t; - t.fromNSec(ts); - return t; -} - } // namespace impl template - #include "ouster_ros/os_sensor_nodelet_base.h" #include #include +#include + #include "ouster_ros/GetMetadata.h" namespace sensor = ouster::sensor; 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 From 6f2495955aa1cf7675755e61985d67ed64d45b81 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 06/10] Make sure all the timestamp values of LidarScan are utc corrected --- include/ouster_ros/os_ros.h | 3 +-- src/lidar_packet_handler.h | 8 +++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 1c2396a6..1eb70146 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -207,7 +207,7 @@ inline ouster::img_t get_or_fill_zero(sensor::ChanField field, } /** - * simple utility to function that ensures we don't wrap around uint64_t due + * 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 */ @@ -215,7 +215,6 @@ 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); diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index c2e27826..a3160abe 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -254,9 +254,11 @@ class LidarPacketHandler { const uint8_t* lidar_buf, int64_t ptp_utc_tai_offset) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; - lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); - lidar_scan_estimated_ts = impl::ts_safe_offset_add( - lidar_scan_estimated_ts, ptp_utc_tai_offset); + auto utc_timestamp = lidar_scan->timestamp().unaryExpr( + [ptp_utc_tai_offset](uint64_t ts) { + return impl::ts_safe_offset_add(ts, ptp_utc_tai_offset); + }); + lidar_scan_estimated_ts = compute_scan_ts(utc_timestamp); lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); return true; From 7d99cfb1489cabba7ae1866dc700a7f085cffaf4 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 07/10] Fix build error --- src/lidar_packet_handler.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index a3160abe..7fac51f0 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -254,10 +254,11 @@ class LidarPacketHandler { const uint8_t* lidar_buf, int64_t ptp_utc_tai_offset) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; - auto utc_timestamp = lidar_scan->timestamp().unaryExpr( - [ptp_utc_tai_offset](uint64_t ts) { - return impl::ts_safe_offset_add(ts, ptp_utc_tai_offset); - }); + ouster::LidarScan::Header utc_timestamp = + lidar_scan->timestamp().unaryExpr( + [ptp_utc_tai_offset](uint64_t ts) { + return impl::ts_safe_offset_add(ts, ptp_utc_tai_offset); + }); lidar_scan_estimated_ts = compute_scan_ts(utc_timestamp); lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); From 3366768c9014adaf3e7d2ac4d5faab423f63b3e5 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 08/10] Modify timestamp values before producing a PointCloud --- src/lidar_packet_handler.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index 7fac51f0..7effc6ba 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -254,12 +254,10 @@ class LidarPacketHandler { const uint8_t* lidar_buf, int64_t ptp_utc_tai_offset) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; - ouster::LidarScan::Header utc_timestamp = - lidar_scan->timestamp().unaryExpr( - [ptp_utc_tai_offset](uint64_t ts) { - return impl::ts_safe_offset_add(ts, ptp_utc_tai_offset); - }); - lidar_scan_estimated_ts = compute_scan_ts(utc_timestamp); + 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; From 730f16182e470cdf27c253fcc2caa0946c9690d5 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:38:15 -0700 Subject: [PATCH 09/10] Refactor the Dockerfile --- Dockerfile | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) 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} From dcbd19390c0d3335e224c6379bd6f946be3660a8 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 24 Aug 2023 09:53:18 -0700 Subject: [PATCH 10/10] Update Changelog and refactor --- CHANGELOG.rst | 1 + src/os_ros.cpp | 9 +++------ 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 88d29958..21794558 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -10,6 +10,7 @@ ouster_ros(1) * 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/src/os_ros.cpp b/src/os_ros.cpp index cf93461a..63e88593 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -199,7 +199,6 @@ void copy_scan_to_cloud_destaggered( const ouster::img_t& 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(); @@ -210,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{