Skip to content

Commit

Permalink
Implement a technique to estimate scan_ts when packets are missing at…
Browse files Browse the repository at this point in the history 
… 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
  • Loading branch information
@Samahu
Samahu authored Mar 7, 2023
1 parent e18781c commit bf86365
Show file tree
Hide file tree
Showing 2 changed files with 157 additions and 17 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
-------------
Expand Down
172 changes: 155 additions & 17 deletions src/os_cloud_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <algorithm>
#include <chrono>
#include <memory>
#include <cassert>

#include "ouster_ros/GetMetadata.h"
#include "ouster_ros/PacketMsg.h"
Expand All @@ -33,6 +34,43 @@ using ouster_ros::PacketMsg;
using sensor::UDPProfileLidar;
using namespace std::chrono_literals;

namespace {

template <typename T, typename UnaryPredicate>
int find_if_reverse(const Eigen::Array<T, -1, 1>& 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 <typename T>
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<uint64_t>(rounded_value);
}

} // namespace

namespace nodelets_os {
class OusterCloud : public nodelet::Nodelet {
private:
Expand All @@ -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);
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<uint64_t>& 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<uint64_t>& 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<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
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()));
Expand Down Expand Up @@ -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<std::chrono::nanoseconds(
const ouster::LidarScan::Header<uint64_t>&)>
compute_scan_ts;
double scan_col_ts_spacing_ns; // interval or spacing between columns of a
// scan
};

} // namespace nodelets_os
Expand Down

0 comments on commit bf86365

Please sign in to comment.