From 057b5bfdfd10ebdeed7a81ea8b603f0e665658ba Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 9 Oct 2024 15:50:25 -0700 Subject: [PATCH 1/4] Don't apply shift to timestamp --- src/point_cloud_compose.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/point_cloud_compose.h b/src/point_cloud_compose.h index 45b0b8aa..37ecc17b 100644 --- a/src/point_cloud_compose.h +++ b/src/point_cloud_compose.h @@ -152,7 +152,7 @@ void scan_to_cloud_f(ouster_ros::Cloud& cloud, cloud.points.emplace_back(); } - auto ts = timestamp[v_shift]; + auto ts = timestamp[v]; ts = ts > scan_ts ? ts - scan_ts : 0UL; // if target point and staging point has matching type bind the From ff1d0d6c214b410770558b24fa7054ff68b78c50 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 10 Oct 2024 14:04:44 -0700 Subject: [PATCH 2/4] align timestamps based on staggered/destaggered option + CHANGELOG + workflows --- .github/ISSUE_TEMPLATE/bug_report.md | 8 ++++++++ .github/ISSUE_TEMPLATE/question.md | 24 ------------------------ CHANGELOG.rst | 10 ++++++++-- src/os_sensor_nodelet.cpp | 4 ++-- src/point_cloud_compose.h | 5 ++++- 5 files changed, 22 insertions(+), 29 deletions(-) delete mode 100644 .github/ISSUE_TEMPLATE/question.md diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index feae2efc..ca57c99a 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -7,6 +7,14 @@ assignees: '' --- +> **IMPORTANT** +> Before reporting a new bug make sure you have: +> - [ ] Checked existing issues https://github.com/ouster-lidar/ouster-ros/issues +> - [ ] Checked existing issues https://github.com/ouster-lidar/ouster-ros/issues +> - [ ] Checked ouster community https://community.ouster.com/ +> If you couldn't find relevant information and you think this is rather a driver +> problem then proceed with bug reporting. + **Describe the bug** A clear and concise description of what the bug is. diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md deleted file mode 100644 index 61a2ea55..00000000 --- a/.github/ISSUE_TEMPLATE/question.md +++ /dev/null @@ -1,24 +0,0 @@ -[IMPORTANT: PLEASE CONSIDER POSTING YOUR QUESTION TO THE ROS TOPIC ON https://forum.ouster.com]: # - ---- -name: Question -about: Do you have a specific question -title: '' -labels: question -assignees: '' - ---- - -**Describe your question** -A clear and concise description of your question. - -**Screenshots** -If applicable, add screenshots to help explain your problem. - -**Platform (please complete the following information):** -- Ouster Sensor? \[e.g. OS-0, OS-1, ..\] -- Ouster Firmware Version? \[e.g. 2.3, 2.4, ..\] -- ROS version/distro? \[e.g. noetic, foxy, humble, ..\] -- Operating System? \[e.g. Linux, Windows, MacOS\] -- Machine Architecture? \[e.g. x64, arm\] -- git commit hash (if not the latest). diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 413b8615..d3f1295e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,14 @@ Changelog ========= -[ouster_ros v0.13.0] -==================== + +[unreleased] +============ +* [BUGFIX]: correctly align timestamps to the generated point cloud + + +ouster_ros v0.13.0 +================== ouster_ros(1) ------------- diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index 57cde7eb..e0d0d521 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -116,9 +116,9 @@ void OusterSensor::onInit() { create_services(); create_publishers(); attempt_reconnect = getPrivateNodeHandle().param("attempt_reconnect", false); - dormant_period_between_reconnects = + dormant_period_between_reconnects = getPrivateNodeHandle().param("dormant_period_between_reconnects", 1.0); - reconnect_attempts_available = + reconnect_attempts_available = getPrivateNodeHandle().param("max_failed_reconnect_attempts", INT_MAX); attempt_start(); } diff --git a/src/point_cloud_compose.h b/src/point_cloud_compose.h index 37ecc17b..00e6b070 100644 --- a/src/point_cloud_compose.h +++ b/src/point_cloud_compose.h @@ -152,7 +152,10 @@ void scan_to_cloud_f(ouster_ros::Cloud& cloud, cloud.points.emplace_back(); } - auto ts = timestamp[v]; + // as opposed to the point cloud destaggering if it is disabled + // then timestamps needs to be staggered. + auto ts = destagger ? + timestamp[v] : timestamp[(v + ls.w + pixel_shift_by_row[u]) % ls.w]; ts = ts > scan_ts ? ts - scan_ts : 0UL; // if target point and staging point has matching type bind the From f33c3c6eba5a9a4412a54708a658eebb23a77db3 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 10 Oct 2024 14:13:26 -0700 Subject: [PATCH 3/4] formatting code --- src/point_cloud_compose.h | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/point_cloud_compose.h b/src/point_cloud_compose.h index 00e6b070..71bd6de2 100644 --- a/src/point_cloud_compose.h +++ b/src/point_cloud_compose.h @@ -3,10 +3,9 @@ #include #include +#include "ouster_ros/common_point_types.h" #include "ouster_ros/os_point.h" #include "ouster_ros/sensor_point_types.h" -#include "ouster_ros/common_point_types.h" - #include "point_meta_helpers.h" #include "point_transform.h" @@ -117,10 +116,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 +130,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,15 +147,15 @@ void scan_to_cloud_f(ouster_ros::Cloud& cloud, } continue; } else { - if (!organized) - cloud.points.emplace_back(); + if (!organized) cloud.points.emplace_back(); } // as opposed to the point cloud destaggering if it is disabled // then timestamps needs to be staggered. - auto ts = destagger ? - timestamp[v] : timestamp[(v + ls.w + pixel_shift_by_row[u]) % ls.w]; - ts = ts > scan_ts ? ts - scan_ts : 0UL; + 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 From 379e768e8901808bac97fd1c4c785b69676fae05 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 10 Oct 2024 14:42:13 -0700 Subject: [PATCH 4/4] Increment package version --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 1a3dd7b2..5d3715d9 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.13.0 + 0.13.1 Ouster ROS driver ouster developers BSD