From bf8636552a775d2739b189da8318d875105545ad Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Tue, 7 Mar 2023 11:54:55 -0800 Subject: [PATCH] Implement a technique to estimate scan_ts when packets are missing at beginning of a lidar scan (#67) * Implement a technique to estimate scan_ts when packets are missing at the beginning of a frame * Be consistent with variable naming * Add an assert statement for find_if_reverse * Extrapolate the time for the first scan * Implement the extrapolation for TIME_FROM_ROS_TIME timestamp option * Address the issue with ulround function not working for integral types * Perform type conversion on the difference * Take always positive difference and compute sign * Use lround instead of invoking cast to integer/long * Update CHANGELOG --- CHANGELOG.rst | 2 + src/os_cloud_nodelet.cpp | 172 +++++++++++++++++++++++++++++++++++---- 2 files changed, 157 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 89a3c363..bbbc181b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,6 +11,8 @@ ouster_ros(1) * bugfix: Address an issue causing the driver to warn about missing non-legacy fields even they exist in the original metadata file. * added a new launch file ``sensor_mtp.launch`` for multicast use case (experimental). +* added a technique to estimate the the value of the lidar scan timestamp when it is missing packets + at the beginning ouster_ros(2) ------------- diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index b86d53b6..d2333248 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include "ouster_ros/GetMetadata.h" #include "ouster_ros/PacketMsg.h" @@ -33,6 +34,43 @@ using ouster_ros::PacketMsg; using sensor::UDPProfileLidar; using namespace std::chrono_literals; +namespace { + +template +int find_if_reverse(const Eigen::Array& array, + UnaryPredicate predicate) { + auto p = array.data() + array.size() - 1; + do { + if (predicate(*p)) return p - array.data(); + } while (p-- != array.data()); + return -1; +} + +uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { + uint64_t min_v, max_v; + double sign; + if (y1 > y0) { + min_v = y0; + max_v = y1; + sign = +1; + } else { + min_v = y1; + max_v = y0; + sign = -1; + } + return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); +} + +template +uint64_t ulround(T value) { + T rounded_value = std::round(value); + if (rounded_value < 0) return 0ULL; + if (rounded_value > ULLONG_MAX) return ULLONG_MAX; + return static_cast(rounded_value); +} + +} // namespace + namespace nodelets_os { class OusterCloud : public nodelet::Nodelet { private: @@ -45,8 +83,12 @@ class OusterCloud : public nodelet::Nodelet { auto& nh = getNodeHandle(); auto metadata = get_metadata(nh); info = sensor::parse_metadata(metadata); - n_returns = compute_n_returns(); + n_returns = compute_n_returns(info.format); + scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); create_lidarscan_objects(); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; create_publishers(nh); create_subscribers(nh); } @@ -77,13 +119,20 @@ class OusterCloud : public nodelet::Nodelet { return request.response.metadata; } - int compute_n_returns() { - return info.format.udp_profile_lidar == + static int compute_n_returns(const sensor::data_format& format) { + return format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL ? 2 : 1; } + static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { + const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); + const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); + const double one_sec_in_ns = 1e+9; + return one_sec_in_ns / (scan_width * scan_frequency); + } + void create_lidarscan_objects() { // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point @@ -152,31 +201,112 @@ class OusterCloud : public nodelet::Nodelet { info.lidar_to_sensor_transform, sensor_frame, lidar_frame, msg_ts)); } - void lidar_handler_sensor_time(const PacketMsg::ConstPtr& packet) { - if (!(*scan_batcher)(packet->buf.data(), ls)) return; - auto ts_v = ls.timestamp(); + uint64_t impute_value(int last_scan_last_nonzero_idx, + uint64_t last_scan_last_nonzero_value, + int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value, + int scan_width) { + assert(scan_width + curr_scan_first_nonzero_idx > + last_scan_last_nonzero_idx); + double interpolated_value = linear_interpolate( + last_scan_last_nonzero_idx, last_scan_last_nonzero_value, + scan_width + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, scan_width); + return ulround(interpolated_value); + } + + uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value) { + double extrapolated_value = + curr_scan_first_nonzero_value - + scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; + return ulround(extrapolated_value); + } + + // compute_scan_ts_0 for first scan + std::chrono::nanoseconds compute_scan_ts_0( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + 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 + : extrapolate_value(curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value); + + 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 + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_n(ts_v); + }; + return std::chrono::nanoseconds(scan_ns); + } + + // compute_scan_ts_n applied to all subsequent scans except first one + std::chrono::nanoseconds compute_scan_ts_n( + const ouster::LidarScan::Header& ts_v) { auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), [](uint64_t h) { return h != 0; }); - if (idx == ts_v.data() + ts_v.size()) return; - auto scan_ts = std::chrono::nanoseconds{ts_v(idx - ts_v.data())}; + 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(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 + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + return std::chrono::nanoseconds(scan_ns); + } + + void lidar_handler_sensor_time(const PacketMsg::ConstPtr& packet) { + if (!(*scan_batcher)(packet->buf.data(), ls)) return; + auto scan_ts = compute_scan_ts(ls.timestamp()); convert_scan_to_pointcloud_publish(scan_ts, to_ros_time(scan_ts)); } + uint16_t packet_col_index(const uint8_t* packet_buf) { + const auto& pf = sensor::get_format(info); + return pf.col_measurement_id(pf.nth_col(0, packet_buf)); + } + + ros::Time extrapolate_frame_ts(const uint8_t* lidar_buf, + const ros::Time current_time) { + auto curr_scan_first_arrived_idx = packet_col_index(lidar_buf); + auto delta_time = ros::Duration( + 0, + std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); + return current_time - delta_time; + } + void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) { auto packet_receive_time = ros::Time::now(); - static auto frame_ts = packet_receive_time; // first point cloud time - if (!(*scan_batcher)(packet->buf.data(), ls)) return; - auto ts_v = ls.timestamp(); - auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), - [](uint64_t h) { return h != 0; }); - if (idx == ts_v.data() + ts_v.size()) return; - auto scan_ts = std::chrono::nanoseconds{ts_v(idx - ts_v.data())}; + const uint8_t* packet_buf = packet->buf.data(); + static auto frame_ts = extrapolate_frame_ts( + packet_buf, packet_receive_time); // first point cloud time + if (!(*scan_batcher)(packet_buf, ls)) return; + auto scan_ts = compute_scan_ts(ls.timestamp()); convert_scan_to_pointcloud_publish(scan_ts, frame_ts); - frame_ts = packet_receive_time; // set time for next point cloud msg + frame_ts = extrapolate_frame_ts( + packet_buf, + packet_receive_time); // set time for next point cloud msg } void imu_handler(const PacketMsg::ConstPtr& packet) { - auto pf = sensor::get_format(info); + const auto& pf = sensor::get_format(info); ros::Time msg_ts = use_ros_time ? ros::Time::now() : to_ros_time(pf.imu_gyro_ts(packet->buf.data())); @@ -224,6 +354,14 @@ class OusterCloud : public nodelet::Nodelet { tf2_ros::TransformBroadcaster tf_bcast; bool use_ros_time; + + int last_scan_last_nonzero_idx = -1; + uint64_t last_scan_last_nonzero_value = 0; + std::function&)> + compute_scan_ts; + double scan_col_ts_spacing_ns; // interval or spacing between columns of a + // scan }; } // namespace nodelets_os