diff --git a/plugins/driver_olei/olei_driver.cpp b/plugins/driver_olei/olei_driver.cpp index 13db359..61b71f4 100644 --- a/plugins/driver_olei/olei_driver.cpp +++ b/plugins/driver_olei/olei_driver.cpp @@ -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(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(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(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg); + float angle = to_signed_deg( + device_deg(static_cast(first_index + i) * angle_inc - 180.f)); angle = maybe_invert(angle, inverted_); if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; diff --git a/plugins/driver_olei/olei_driver.hpp b/plugins/driver_olei/olei_driver.hpp index d62f714..bd4c916 100644 --- a/plugins/driver_olei/olei_driver.hpp +++ b/plugins/driver_olei/olei_driver.hpp @@ -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_;