refactor: restructure lidarlib into xlidar-driver plugin SDK

- LidarManager facade (liblidar_manager.so): dlopen plugin discovery,
  available_drivers map<driver_id, PluginRegistry>, create_lidar_device,
  config.json load/save with legacy lidarlib migration
- Common LidarDriverInterface + DriverInfo/DeviceConfig plugin ABI
  (extern C get_driver_info / create_driver_instance)
- Plugins: driver_rplidar (ported from xlocd, Slamtec SDK), driver_olei,
  driver_sick_code (TiM CoLa-A), driver_sick_safety (nanoScan3), driver_espe
- Diagnostics extended with rplidar health + firmware; FOV filter window,
  range override and legacy remap window unified in DeviceConfig
- Rewritten README, diagnostics doc and examples (list_drivers, example,
  lidar_app)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
2026-07-12 22:30:56 +07:00
parent 49d4e04530
commit 5b2c74bd36
43 changed files with 2346 additions and 1556 deletions

View File

@@ -0,0 +1,457 @@
// OLEI 2D lidars over UDP — Family A (0xFAF0), Family B (0xFEF0) and
// Family C / Protocol V3 (0xFEAC, GS1-5) packet parsing.
#include "olei_driver.hpp"
#include "plugin_helpers.hpp"
#include <cerrno>
#include <cstring>
#include <cmath>
#include <arpa/inet.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <unistd.h>
namespace xlidar {
// Normalize into (-180, 180]: 0 = ahead, + = left, - = right.
static inline float to_signed_deg(float deg) {
deg = std::fmod(deg, 360.f);
if (deg < 0.f) deg += 360.f;
if (deg > 180.f) deg -= 360.f;
return deg;
}
static inline float maybe_invert(float signed_deg, bool inverted) {
return inverted ? to_signed_deg(-signed_deg) : signed_deg;
}
// CRC32 poly 0x04C11DB7, MSB-first
static uint32_t crc32_olei(const uint8_t* data, size_t len) {
uint32_t crc = 0xFFFFFFFF;
for (size_t i = 0; i < len; ++i) {
crc ^= static_cast<uint32_t>(data[i]) << 24;
for (int b = 0; b < 8; ++b)
crc = (crc & 0x80000000u) ? (crc << 1) ^ 0x04C11DB7u : (crc << 1);
}
return crc;
}
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)
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);
}
OleiDriver::~OleiDriver() { close(); }
ErrorCode OleiDriver::open() {
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
return set_error(ErrorCode::InvalidAddress);
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
#ifdef SO_REUSEPORT
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
#endif
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
int err = errno;
::close(sock_fd_);
sock_fd_ = -1;
return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
: ErrorCode::BindFailed);
}
// Reset per-revolution state so a close()/open() cycle starts clean.
pending_angle_deg_.clear();
pending_dist_m_.clear();
pending_intensity_.clear();
pending_info_ = ExtraInfo{};
latest_diag_ = Diagnostics{};
last_angle_ = -1.f;
scan_ready_ = false;
pending_angle_deg_.reserve(2048);
pending_dist_m_.reserve(2048);
pending_intensity_.reserve(2048);
return set_error(ErrorCode::Ok);
}
void OleiDriver::close() {
if (sock_fd_ >= 0) {
::close(sock_fd_);
sock_fd_ = -1;
}
}
bool OleiDriver::recv_scan(ScanResult& out, int timeout_ms) {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
scan_ready_ = false;
while (!scan_ready_) {
if (timeout_ms > 0) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) {
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
return false;
}
}
if (!poll_packet()) return false;
}
out = std::move(ready_result_);
set_error(ErrorCode::Ok);
return true;
}
bool OleiDriver::spin_once() {
if (!poll_packet()) return false;
if (scan_ready_) {
scan_ready_ = false;
if (cb_) cb_(ready_result_);
}
return true;
}
bool OleiDriver::poll_packet() {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
uint8_t* buf = recv_buf_;
sockaddr_in from{};
socklen_t fromlen = sizeof(from);
ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
reinterpret_cast<sockaddr*>(&from), &fromlen);
if (n < 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
// A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
if (n < 4) return true;
uint16_t id_at_0 = le16(buf);
uint16_t frame_id_b = le16(buf + 2);
if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast<int>(n));
else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast<int>(n));
else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast<int>(n));
return true;
}
// Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
void OleiDriver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
float angle = signed_angle_deg;
if (!pending_angle_deg_.empty()) {
float prev = pending_angle_deg_.back();
while (angle - prev > 180.f) angle -= 360.f;
while (angle - prev < -180.f) angle += 360.f;
}
pending_angle_deg_.push_back(angle);
pending_dist_m_.push_back(dist_m);
pending_intensity_.push_back(intensity);
}
void OleiDriver::flush_scan() {
if (pending_angle_deg_.empty()) return;
const size_t n = pending_angle_deg_.size();
LaserScan& scan = ready_result_.scan;
scan.timestamp_ms = pending_ts_;
scan.angle_min = pending_angle_deg_.front() * kDeg2Rad;
scan.angle_max = pending_angle_deg_.back() * kDeg2Rad;
scan.angle_increment = (n > 1)
? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f;
scan.time_increment = 0.f;
scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end());
// Inversion already happened per point (maybe_invert), so inverted=false.
finalize_scan(scan, cfg_, /*inverted=*/false);
ExtraInfo& info = ready_result_.info;
info = pending_info_;
info.detected_model = detected_model_name_;
info.error_status = pending_err_;
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
mark_scan_decoded();
pending_angle_deg_.clear();
pending_dist_m_.clear();
pending_intensity_.clear();
pending_info_ = ExtraInfo{};
scan_ready_ = true;
}
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
bool OleiDriver::parse_family_a(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 20;
static constexpr int BLOCK_LEN = 3;
if (len < HEADER_LEN) return false;
uint8_t dist_scale = buf[4]; // mm per count
uint8_t err_status = buf[5];
float ang_start = static_cast<float>(buf[6]);
uint16_t num_pts = le16(buf + 8);
uint16_t rotation_raw = le16(buf + 10);
uint32_t timestamp = le32(buf + 12);
uint32_t crc_packet = le32(buf + 16);
int block_bytes = len - HEADER_LEN;
if (block_bytes < num_pts * BLOCK_LEN) return false;
uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<size_t>(num_pts * BLOCK_LEN));
if (crc_calc != crc_packet) return false;
if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
flush_scan();
}
pending_ts_ = timestamp;
pending_err_ = err_status;
pending_info_.distance_scale_mm = dist_scale;
pending_info_.rotation_raw = rotation_raw;
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
const float ang_end = static_cast<float>(buf[7]);
const uint8_t* blk = buf + HEADER_LEN;
for (uint16_t i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
uint16_t dist_raw = le16(blk);
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);
angle = maybe_invert(angle, inverted_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
push_point(angle, dist_raw * scale_mm * 0.001f, intensity);
}
last_angle_ = ang_start;
return true;
}
// Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks
// (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire.
bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 40;
static constexpr int BLOCK_LEN = 8;
if (len < HEADER_LEN) return false;
uint8_t dist_scale = buf[6];
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
pending_info_.distance_scale_mm = dist_scale;
if (auto_detect_ && !model_locked_) {
std::string raw(reinterpret_cast<const char*>(buf + 7), 10);
size_t z = raw.find('\0');
if (z != std::string::npos) raw.resize(z);
if (!raw.empty()) {
detected_model_name_ = raw;
model_locked_ = true;
static constexpr struct { const char* key; const ModelConfig* cfg; } kModelTable[] = {
{ "1BS5", &MODEL_LR1BS5 },
{ "16F", &MODEL_LR16F },
{ "1FMI", &MODEL_LR1FMI }, // must precede "1F": "OLELR-1FMI" also contains "1F"
{ "1F", &MODEL_LR1F },
{ "VF", &MODEL_VF },
{ "VB", &MODEL_VB },
};
for (const auto& entry : kModelTable) {
if (raw.find(entry.key) != std::string::npos) {
cfg_.scan_angle_min = entry.cfg->scan_angle_min;
cfg_.scan_angle_max = entry.cfg->scan_angle_max;
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;
break;
}
}
}
}
int num_pts = (len - HEADER_LEN) / BLOCK_LEN;
if (num_pts <= 0) return false;
const uint8_t* blk = buf + HEADER_LEN;
// A packet is only a ~22° arc and may span >1 rev, so the revolution
// boundary is detected per point: a >90° drop between consecutive angles.
static constexpr uint16_t INVALID_ANGLE = 0xFF00;
for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
uint16_t angle_raw = le16(blk);
if (angle_raw >= INVALID_ANGLE) continue;
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f);
if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
flush_scan();
}
last_angle_ = dev_deg;
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), 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
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
push_point(angle, dist_m, intensity);
}
return true;
}
// Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending
// on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real
// hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180.
bool OleiDriver::parse_family_c(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 48;
if (len < HEADER_LEN) return false;
uint16_t header_size_field = le16(buf + 8);
uint8_t distance_ratio_raw = buf[10];
uint8_t types = buf[11];
uint16_t scan_frequency_raw = le16(buf + 24);
uint16_t num_pts_scan = le16(buf + 26);
uint16_t input_status = le16(buf + 28);
uint16_t output_status = le16(buf + 30);
uint32_t field_status = le32(buf + 32);
uint16_t first_index = le16(buf + 40);
uint16_t num_pts_packet = le16(buf + 42);
uint32_t status_flags = le32(buf + 44);
if (num_pts_scan == 0) return false;
int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
if (header_size < HEADER_LEN || header_size > len) return false;
// Types: 0x00 = 2B/point (range only), 0x01 = 4B (range+intensity),
// 0x10 = 4B (range at [+2,+4)).
int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
if (bytes_per_point == 0) return false;
int payload_bytes = len - header_size;
int num_pts = num_pts_packet;
if (num_pts == 0 || num_pts * bytes_per_point > payload_bytes) {
num_pts = payload_bytes / bytes_per_point;
}
if (num_pts <= 0) return false;
pending_info_.distance_ratio_raw = distance_ratio_raw;
pending_info_.scan_frequency_raw = scan_frequency_raw;
pending_info_.input_status = input_status;
pending_info_.output_status = output_status;
pending_info_.field_status = field_status;
pending_info_.status_flags = status_flags;
// Magic 0xFEAC == exactly one model (GS1-5).
if (auto_detect_ && !model_locked_) {
cfg_.scan_angle_min = MODEL_GS15.scan_angle_min;
cfg_.scan_angle_max = MODEL_GS15.scan_angle_max;
cfg_.range_min_m = MODEL_GS15.range_min_m;
cfg_.range_max_m = MODEL_GS15.range_max_m;
cfg_.angle_offset_deg = MODEL_GS15.angle_offset_deg;
detected_model_name_ = MODEL_GS15.name;
model_locked_ = true;
}
const float angle_inc = 360.f / static_cast<float>(num_pts_scan);
float raw_first_angle = static_cast<float>(first_index) * angle_inc;
if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) {
flush_scan();
}
const uint8_t* blk = buf + header_size;
for (int i = 0; i < num_pts; ++i, blk += bytes_per_point) {
uint16_t range_mm;
uint16_t inten_raw = 0;
bool has_inten = false;
if (types == 0x00) {
range_mm = le16(blk);
} else if (types == 0x01) {
range_mm = le16(blk);
inten_raw = le16(blk + 2);
has_inten = true;
} else { // 0x10
range_mm = le16(blk + 2);
}
float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg);
angle = maybe_invert(angle, inverted_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
push_point(angle, range_mm * 0.001f,
has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
}
last_angle_ = raw_first_angle;
return true;
}
// ── plugin registration ─────────────────────────────────────────────────────
namespace {
const DriverInfo kDriverInfo = [] {
DriverInfo info;
info.vendor = "OLEI";
info.model = "2D series (VB/VF/LR-1x/GS1-5)";
info.driver_id = "olei_lidar_driver";
info.description = "OLEI 2D lidars over UDP — auto-detects the Family A/B/C "
"protocol per packet; model AUTO self-detects from the "
"stream (Family B/C). Default port 2368.";
info.transport = Transport::Udp;
info.supported_models = {"AUTO", "VB", "VF", "LR-1F", "LR-1FMI", "LR-1BS5",
"LR-16F", "GS1-5"};
return info;
}();
const ModelConfig* model_by_name(const std::string& name) {
static constexpr const ModelConfig* kModels[] = {
&MODEL_AUTO, &MODEL_VB, &MODEL_VF, &MODEL_LR1F, &MODEL_LR1FMI,
&MODEL_LR1BS5, &MODEL_LR16F, &MODEL_GS15,
};
for (const ModelConfig* m : kModels)
if (name == m->name) return m;
return nullptr;
}
} // namespace
DriverInfo OleiDriver::get_driver_info() const { return kDriverInfo; }
} // namespace xlidar
XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
*out = xlidar::kDriverInfo;
}
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
create_driver_instance(const xlidar::DeviceConfig* cfg) {
using namespace xlidar;
const ModelConfig* preset = model_by_name(cfg->model);
if (!preset) preset = &MODEL_AUTO; // unknown model → auto-detect
const uint16_t port = cfg->port ? cfg->port : 2368;
return new OleiDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
}