diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ea55f61b..135d4bed 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog ========= +[unreleased] +============ +* [BUGFIX]: correctly align timestamps to the generated point cloud. + ouster_ros v0.13.2 ================== * [BUGFIX]: Make sure to initialize the sensor with launch file parameters. diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index b00538b4..a2a1bd15 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.13.2 + 0.13.3 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/point_cloud_compose.h b/ouster-ros/src/point_cloud_compose.h index 6b425438..2e4895cd 100644 --- a/ouster-ros/src/point_cloud_compose.h +++ b/ouster-ros/src/point_cloud_compose.h @@ -117,10 +117,9 @@ using Cloud = pcl::PointCloud; // TODO[UN]: make this a functor template & PROFILE, typename PointT, typename PointS> -void scan_to_cloud_f(ouster_ros::Cloud& cloud, - PointS& staging_point, - const ouster::PointsF& points, - uint64_t scan_ts, const ouster::LidarScan& ls, +void scan_to_cloud_f(ouster_ros::Cloud& cloud, PointS& staging_point, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, const std::vector& pixel_shift_by_row, bool organized = false, bool destagger = true, int rows_step = 1) { @@ -132,11 +131,12 @@ void scan_to_cloud_f(ouster_ros::Cloud& cloud, for (auto u = 0; u < ls.h; u += rows_step) { for (auto v = 0; v < ls.w; ++v) { // TODO[UN]: consider cols_step in future - const auto v_shift = destagger ? - (v + ls.w - pixel_shift_by_row[u]) % ls.w : v; + const auto v_shift = + destagger ? (v + ls.w - pixel_shift_by_row[u]) % ls.w : v; const auto src_idx = u * ls.w + v_shift; const auto xyz = points.row(src_idx); - const auto tgt_idx = organized ? (u / rows_step) * ls.w + v : cloud.size(); + const auto tgt_idx = + organized ? (u / rows_step) * ls.w + v : cloud.size(); if (xyz.isNaN().any()) { if (organized) { @@ -148,12 +148,15 @@ void scan_to_cloud_f(ouster_ros::Cloud& cloud, } continue; } else { - if (!organized) - cloud.points.emplace_back(); + if (!organized) cloud.points.emplace_back(); } - auto ts = timestamp[v_shift]; - ts = ts > scan_ts ? ts - scan_ts : 0UL; + // as opposed to the point cloud destaggering if it is disabled + // then timestamps needs to be staggered. + auto ts_idx = + destagger ? v : (v + ls.w + pixel_shift_by_row[u]) % ls.w; + auto ts = + timestamp[ts_idx] > scan_ts ? timestamp[ts_idx] - scan_ts : 0UL; // if target point and staging point has matching type bind the // target directly and avoid performing transform_point at the end