From abf7bab917f7694933e220b77d8b03d1e1523d92 Mon Sep 17 00:00:00 2001 From: loctv Date: Thu, 23 Jul 2026 11:17:08 +0700 Subject: [PATCH] =?UTF-8?q?fix(rplidar):=20Slamtec=20angle=20scale=20is=20?= =?UTF-8?q?clockwise=20=E2=80=94=20swap=20the=20mirror=20branches?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Slamtec devices count their angle clockwise viewed from the top (Interface Protocol spec; the official rplidar_ros node mirrors the angles for the same reason), but the decode used the raw ascending angles as CCW for a right-side-up unit and mirrored them for an upside-down one — exactly backwards. Field-verified 2026-07-23: a right-side-up C1 produced a left-right mirrored sweep against a verified right-handed reference lidar. Now: right-side-up mirrors (angle' = 2π − raw, nodes walked backwards); upside-down uses the raw ascending angles, because the physical flip already reverses the apparent rotation. The mountedUpsideDown flag finally carries its true physical meaning. Co-Authored-By: Claude Opus 4.8 (1M context) --- plugins/driver_rplidar/rplidar_driver.cpp | 33 ++++++++++++++++------- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/plugins/driver_rplidar/rplidar_driver.cpp b/plugins/driver_rplidar/rplidar_driver.cpp index e2736f3..5554933 100644 --- a/plugins/driver_rplidar/rplidar_driver.cpp +++ b/plugins/driver_rplidar/rplidar_driver.cpp @@ -2,9 +2,10 @@ // (third_party/rplidar_sdk, sl_lidar.h). Scan math: angle/distance decoding, // inversion, FOV window, invalid points as NaN. // -// Unlike the network drivers, angles are reported in the DEVICE frame -// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData -// yields. +// Unlike the network drivers, angles arrive in the DEVICE frame [0, 2π), +// 0 = ahead, ascending (the SDK's ascendScanData order) — but Slamtec's +// angle scale runs CLOCKWISE viewed from the top, so decode mirrors it into +// our right-handed convention (see the handedness comment in recv_scan). #include "lidar_interface.hpp" #include "plugin_helpers.hpp" @@ -199,14 +200,25 @@ public: LaserScan& scan = out.scan; scan = LaserScan{}; - // Inverted mount -> mirror the angles (angle' = 2π - angle) and walk - // the nodes backwards to keep ascending order. + // Handedness: Slamtec devices count their angle CLOCKWISE viewed from + // the top (Interface Protocol spec; the official rplidar_ros node + // mirrors the angles for exactly this reason). Our output convention + // is right-handed (CCW, + = left), so: + // - right-side-up (inverted_ == false): mirror (angle' = 2π − raw) + // and walk the nodes backwards to keep ascending order; + // - upside-down (inverted_ == true): the flip makes the rotation + // appear CCW from above, so the raw ascending angles are already + // right-handed — use them as-is. + // Field-verified (2026-07-23): a right-side-up C1 decoded as CCW + // produced a left-right mirrored sweep against a verified + // right-handed reference lidar; invisible with the C1 alone because + // a single-sensor SLAM map is self-consistently mirrored. if (inverted_) { - scan.angle_min = kTwoPi - angle_last; - scan.angle_max = kTwoPi - angle_first; - } else { scan.angle_min = angle_first; scan.angle_max = angle_last; + } else { + scan.angle_min = kTwoPi - angle_last; + scan.angle_max = kTwoPi - angle_first; } scan.angle_increment = (scan.angle_max - scan.angle_min) / static_cast(count - 1); scan.scan_time = scan_time; @@ -221,7 +233,10 @@ public: scan.ranges.reserve(count); scan.intensities.reserve(count); for (std::size_t i = 0; i < count; ++i) { - const std::size_t node_index = inverted_ ? count - 1 - i : i; + // Mirrored branches walk backwards (see the handedness comment + // above): backwards for a right-side-up unit, forwards when the + // physical flip already reversed the apparent rotation. + const std::size_t node_index = inverted_ ? i : count - 1 - i; // dist = 0 is the SDK's "no return" sentinel; together with // out-of-range / out-of-window points it becomes NaN. const float distance = node_distance_m(nodes[node_index]);