QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(rplidar): Slamtec angle scale is clockwise — swap the mirror branches

Browse Source 
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) <noreply@anthropic.com>
This commit is contained in:
Trương Văn Lộc
2026-07-23 11:17:08 +07:00
parent 4aa110cde1
commit abf7bab917
1 changed files with 24 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
plugins/driver_rplidar/rplidar_driver.cpp
View File

@@ -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<float>(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]);