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

fix(olei): Family B angle scale is clockwise — mirror the decode

Browse Source 
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>
This commit is contained in:
Trương Văn Lộc
2026-07-23 11:17:08 +07:00
parent 553b07147b
commit 4aa110cde1
2 changed files with 43 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
plugins/driver_olei/olei_driver.cpp
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_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,
bool inverted)
: cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
{
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(); }
@@ -238,7 +268,7 @@ bool OleiDriver::parse_family_a(const uint8_t* buf, int len) {
uint8_t intensity = blk[2];
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_);
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_max_m = entry.cfg->range_max_m;
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
model_mirror_ = model_angles_clockwise(entry.cfg->name);
break;
}
}
@@ -308,7 +339,7 @@ bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
}
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;
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);
}
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_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;

9
plugins/driver_olei/olei_driver.hpp
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_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_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_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 };
@@ -54,11 +54,18 @@ private:
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
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_;
std::string ip_;
uint16_t port_;
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;
ScanCallback cb_;