loctv
4aa110cde1
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>
97 lines
4.3 KiB
C++
97 lines
4.3 KiB
C++
// OLEI 2D lidars over UDP (Family A / B / C protocols) — plugin-private header.
|
||
#pragma once
|
||
#include "lidar_interface.hpp"
|
||
|
||
#include <string>
|
||
#include <vector>
|
||
|
||
namespace xlidar {
|
||
|
||
// Model presets. Family B/C packets carry enough to auto-detect the model;
|
||
// Family A doesn't, so MODEL_AUTO keeps the wide default FOV.
|
||
inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270°
|
||
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 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 };
|
||
|
||
// OLEI UDP driver.
|
||
class OleiDriver : public LidarDriverInterface {
|
||
public:
|
||
// ip: local bind address; port: UDP port the lidar sends to;
|
||
// inverted: unit mounted upside-down → mirror every angle.
|
||
explicit OleiDriver(const ModelConfig& cfg,
|
||
const std::string& ip = "0.0.0.0",
|
||
uint16_t port = 2368,
|
||
bool inverted = false);
|
||
~OleiDriver();
|
||
|
||
OleiDriver(const OleiDriver&) = delete;
|
||
OleiDriver& operator=(const OleiDriver&) = delete;
|
||
|
||
DriverInfo get_driver_info() const override;
|
||
|
||
ErrorCode open() override;
|
||
void close() override;
|
||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||
bool spin_once() override;
|
||
bool is_open() const override { return sock_fd_ >= 0; }
|
||
|
||
// Model name read from the Family B/C header; "AUTO" until one is seen.
|
||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||
|
||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||
|
||
private:
|
||
bool poll_packet(); // one recvfrom() + dispatch to the family parser
|
||
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
|
||
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
|
||
bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5)
|
||
|
||
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_;
|
||
|
||
// Per-revolution accumulation buffers (index-aligned)
|
||
std::vector<float> pending_angle_deg_;
|
||
std::vector<float> pending_dist_m_;
|
||
std::vector<uint8_t> pending_intensity_;
|
||
uint32_t pending_ts_ = 0;
|
||
uint8_t pending_err_ = 0;
|
||
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
||
|
||
ExtraInfo pending_info_;
|
||
|
||
// Snapshot for get_diagnostics(); refreshed by flush_scan().
|
||
Diagnostics latest_diag_;
|
||
|
||
ScanResult ready_result_;
|
||
bool scan_ready_ = false;
|
||
|
||
// Per-instance so two drivers on two threads don't race.
|
||
uint8_t recv_buf_[4096];
|
||
|
||
bool auto_detect_ = false;
|
||
bool model_locked_ = false;
|
||
std::string detected_model_name_ = "AUTO";
|
||
};
|
||
|
||
} // namespace xlidar
|