fix(rplidar): Slamtec angle scale is clockwise — swap the mirror branches
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:
@@ -2,9 +2,10 @@
|
|||||||
// (third_party/rplidar_sdk, sl_lidar.h). Scan math: angle/distance decoding,
|
// (third_party/rplidar_sdk, sl_lidar.h). Scan math: angle/distance decoding,
|
||||||
// inversion, FOV window, invalid points as NaN.
|
// inversion, FOV window, invalid points as NaN.
|
||||||
//
|
//
|
||||||
// Unlike the network drivers, angles are reported in the DEVICE frame
|
// Unlike the network drivers, angles arrive in the DEVICE frame [0, 2π),
|
||||||
// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData
|
// 0 = ahead, ascending (the SDK's ascendScanData order) — but Slamtec's
|
||||||
// yields.
|
// 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 "lidar_interface.hpp"
|
||||||
#include "plugin_helpers.hpp"
|
#include "plugin_helpers.hpp"
|
||||||
|
|
||||||
@@ -199,14 +200,25 @@ public:
|
|||||||
|
|
||||||
LaserScan& scan = out.scan;
|
LaserScan& scan = out.scan;
|
||||||
scan = LaserScan{};
|
scan = LaserScan{};
|
||||||
// Inverted mount -> mirror the angles (angle' = 2π - angle) and walk
|
// Handedness: Slamtec devices count their angle CLOCKWISE viewed from
|
||||||
// the nodes backwards to keep ascending order.
|
// 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_) {
|
if (inverted_) {
|
||||||
scan.angle_min = kTwoPi - angle_last;
|
|
||||||
scan.angle_max = kTwoPi - angle_first;
|
|
||||||
} else {
|
|
||||||
scan.angle_min = angle_first;
|
scan.angle_min = angle_first;
|
||||||
scan.angle_max = angle_last;
|
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.angle_increment = (scan.angle_max - scan.angle_min) / static_cast<float>(count - 1);
|
||||||
scan.scan_time = scan_time;
|
scan.scan_time = scan_time;
|
||||||
@@ -221,7 +233,10 @@ public:
|
|||||||
scan.ranges.reserve(count);
|
scan.ranges.reserve(count);
|
||||||
scan.intensities.reserve(count);
|
scan.intensities.reserve(count);
|
||||||
for (std::size_t i = 0; i < count; ++i) {
|
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
|
// dist = 0 is the SDK's "no return" sentinel; together with
|
||||||
// out-of-range / out-of-window points it becomes NaN.
|
// out-of-range / out-of-window points it becomes NaN.
|
||||||
const float distance = node_distance_m(nodes[node_index]);
|
const float distance = node_distance_m(nodes[node_index]);
|
||||||
|
|||||||
Reference in New Issue
Block a user