Compare commits

..

3 Commits

Author SHA1 Message Date
145a647d35 docs(interface): document the CCW angle-convention contract on LaserScan
Every driver must normalize its device's native scale into ROS REP-103
right-handed CCW (0 = front, + = left). Spells out the three stackable
device quirks to absorb — shifted 0° reference, clockwise scale,
upside-down mounting — and warns that a CW scale decoded as CCW mirrors
the world invisibly on single-lidar devices: verify handedness against
the real room, not just against motion.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-07-23 11:17:08 +07:00
abf7bab917 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>
2026-07-23 11:17:08 +07:00
4aa110cde1 fix(olei): Family B angle scale is clockwise — mirror the decode
The LR-1BS5's angle scale runs clockwise viewed from the top, so
decoding it as CCW mirrored the whole world left/right — undetectable
with a single lidar (the SLAM map is self-consistently mirrored and
heading-vs-motion checks pass, since a mirror about x preserves
'ahead'). It surfaced when a second, right-handed lidar disagreed
(doubled walls), and the finished map came out mirrored versus the
actual room.

device_deg() now negates the raw angle BEFORE the model's 0°-reference
offset (LR-1BS5: out = 180 − raw), driven by model_angles_clockwise().
Evidence: field-verified on OLELR-1BS5 (2026-07-23), and DF Automation's
production ROS driver (github.com/dfautomation/ole2d, decoder.cpp)
walks the device array backwards with the comment 'reverse, laserscan
is anticlockwise' — its packet layout is exactly our Family B, so
LR-1FMI (same protocol) is listed too. Family A/C stay CCW until a
unit is verified against a real room.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-07-23 11:17:08 +07:00
4 changed files with 82 additions and 13 deletions

View File

@@ -79,6 +79,21 @@ inline const char* to_string(ErrorCode e) {
// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds). // ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
// ranges[i] is at angle_min + i*angle_increment, in sweep order. // ranges[i] is at angle_min + i*angle_increment, in sweep order.
//
// ANGLE CONVENTION CONTRACT (ROS REP-103, right-handed): 0 = device front,
// positive = LEFT, increasing counter-clockwise viewed from the top. Every
// driver must NORMALIZE its device's native scale into this frame — three
// independent, stackable device quirks to absorb:
// 1. shifted 0° reference -> ModelConfig::angle_offset_deg
// (OLEI LR-1F/1FMI/1BS5: 0° at the rear; SICK TiM: front at 90°);
// 2. CLOCKWISE angle scale -> mirror the raw angle (θ -> −θ) BEFORE
// the offset (all Slamtec RPLIDARs per the Interface Protocol spec;
// OLEI LR-1BS5, field-verified). Decoding a CW scale as CCW mirrors the
// whole world left/right — undetectable with a single lidar (the SLAM
// map is self-consistently mirrored and heading checks still pass), so
// verify handedness against the real room, not just against motion;
// 3. upside-down mounting -> the physical flip reverses the
// apparent rotation, one more angle negation (the driver's `inverted`).
struct LaserScan { struct LaserScan {
uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
float angle_min = 0.f; // rad float angle_min = 0.f; // rad

View File

@@ -40,11 +40,41 @@ static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F)
static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant
static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5) static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5)
// Models whose angle scale runs CLOCKWISE (left-handed): decoding them as CCW
// mirrors the world left/right. Undetectable with a single lidar (the SLAM map
// is self-consistently mirrored, and heading-vs-motion checks pass because a
// mirror about x preserves "ahead") — it only surfaces when a second,
// right-handed lidar disagrees, or when the map is compared against the real
// room.
//
// Evidence:
// - LR-1BS5: field-verified 2026-07-23 (rotation-only decode produced a
// left-right mirrored map versus the actual room; mirrored decode matches).
// - The Family B azimuth protocol itself is clockwise per DF Automation's
// production ROS driver (github.com/dfautomation/ole2d,
// src/olelidar/src/decoder.cpp: walks the device array backwards with the
// comment "reverse, laserscan is anticlockwise"; its packet layout —
// azimuth x0.01 deg, invalid sentinel 0xFF00 — is exactly our Family B).
// LR-1FMI speaks the same Family B protocol, so it is listed too.
// - Family A (VB/VF/LR-1F) and Family C (GS1-5) units are unverified; they
// stay CCW until checked against a real room.
static bool model_angles_clockwise(const char* name) {
return std::strcmp(name, "LR-1BS5") == 0 || std::strcmp(name, "LR-1FMI") == 0;
}
OleiDriver::OleiDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port, OleiDriver::OleiDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
bool inverted) bool inverted)
: cfg_(cfg), ip_(ip), port_(port), inverted_(inverted) : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
{ {
auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0); auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
model_mirror_ = model_angles_clockwise(cfg.name);
}
// Device angle -> our CCW convention: clockwise models get their raw angle
// negated BEFORE the model's 0°-reference offset is added (LR-1BS5: 0° at the
// rear AND clockwise, so out = 180 raw).
float OleiDriver::device_deg(float raw_deg) const {
return (model_mirror_ ? -raw_deg : raw_deg) + cfg_.angle_offset_deg;
} }
OleiDriver::~OleiDriver() { close(); } OleiDriver::~OleiDriver() { close(); }
@@ -238,7 +268,7 @@ bool OleiDriver::parse_family_a(const uint8_t* buf, int len) {
uint8_t intensity = blk[2]; uint8_t intensity = blk[2];
float frac = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f; float frac = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg); float angle = to_signed_deg(device_deg(ang_start + frac * (ang_end - ang_start)));
angle = maybe_invert(angle, inverted_); angle = maybe_invert(angle, inverted_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
@@ -285,6 +315,7 @@ bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
cfg_.range_min_m = entry.cfg->range_min_m; cfg_.range_min_m = entry.cfg->range_min_m;
cfg_.range_max_m = entry.cfg->range_max_m; cfg_.range_max_m = entry.cfg->range_max_m;
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg; cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
model_mirror_ = model_angles_clockwise(entry.cfg->name);
break; break;
} }
} }
@@ -308,7 +339,7 @@ bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
} }
last_angle_ = dev_deg; last_angle_ = dev_deg;
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_); float angle = maybe_invert(to_signed_deg(device_deg(angle_raw * 0.01f)), inverted_);
float dist_m = le16(blk + 2) * scale_mm * 0.001f; float dist_m = le16(blk + 2) * scale_mm * 0.001f;
uint8_t intensity = static_cast<uint8_t>(le16(blk + 4) >> 2); // 10-bit → 8-bit uint8_t intensity = static_cast<uint8_t>(le16(blk + 4) >> 2); // 10-bit → 8-bit
@@ -397,7 +428,8 @@ bool OleiDriver::parse_family_c(const uint8_t* buf, int len) {
range_mm = le16(blk + 2); range_mm = le16(blk + 2);
} }
float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg); float angle = to_signed_deg(
device_deg(static_cast<float>(first_index + i) * angle_inc - 180.f));
angle = maybe_invert(angle, inverted_); angle = maybe_invert(angle, inverted_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;

View File

@@ -13,7 +13,7 @@ inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.
inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear
inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear (field-verified on OLELR-1BS5: with offset 0 the SLAM heading came out 180° from the direction of travel) inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear AND the angle scale runs CLOCKWISE — the driver mirrors it (out = 180 raw, see model_angles_clockwise). Field-verified on OLELR-1BS5: offset 0 flipped the SLAM heading, rotation-only 180 then left-right mirrored the whole map.
inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line
inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family C/V3) inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family C/V3)
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f }; inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
@@ -54,11 +54,18 @@ private:
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity); void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
void flush_scan(); void flush_scan();
// Raw device angle -> our CCW convention (negated for clockwise-scale
// models, then the model's 0°-reference offset).
float device_deg(float raw_deg) const;
ModelConfig cfg_; ModelConfig cfg_;
std::string ip_; std::string ip_;
uint16_t port_; uint16_t port_;
bool inverted_ = false; bool inverted_ = false;
// Model's angle scale runs clockwise (e.g. LR-1BS5): device_deg() negates
// the raw angle so the output is right-handed. Set from the preset in the
// ctor and again when AUTO locks onto a detected model.
bool model_mirror_ = false;
int sock_fd_ = -1; int sock_fd_ = -1;
ScanCallback cb_; ScanCallback cb_;

View File

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