Compare commits
3 Commits
553b07147b
...
145a647d35
| Author | SHA1 | Date | |
|---|---|---|---|
| 145a647d35 | |||
| abf7bab917 | |||
| 4aa110cde1 |
@@ -79,6 +79,21 @@ inline const char* to_string(ErrorCode e) {
|
||||
|
||||
// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
|
||||
// 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 {
|
||||
uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
|
||||
float angle_min = 0.f; // rad
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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_;
|
||||
|
||||
|
||||
@@ -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]);
|
||||
|
||||
Reference in New Issue
Block a user