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

Refactor lidar library: rename olei_config to lidar_config, add nanoscan example and shared byte helpers

Browse Source 
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
QUYVN
2026-07-03 09:27:43 +07:00
parent 59880871b0
commit 323715eab0
16 changed files with 621 additions and 814 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
include/lidarlib/lidar.hpp
View File

@@ -7,45 +7,31 @@
namespace lidarlib {
// ─── Default output: ROS sensor_msgs/LaserScan-shaped ──────────────────────
// Same field names/semantics as ROS's LaserScan message (radians, meters,
// seconds) so this can be bridged into a ROS node with a near-1:1 field copy.
// ranges[i]/intensities[i] correspond to angle = angle_min + i*angle_increment;
// the array spans exactly one revolution (or the model's FOV window) in the
// order the device actually swept it — angle_min/angle_max are NOT clamped to
// [-pi,pi], they just describe whatever contiguous window this revolution
// covered (matches how continuously-rotating lidars without a phase reset
// behave: the starting angle drifts slightly scan to scan).
// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
// ranges[i] is at angle_min + i*angle_increment, in sweep order.
struct LaserScan {
uint32_t timestamp_ms = 0; // device clock (ms since power-on); 0 if the
// family doesn't expose one (see ExtraInfo)
uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
float angle_min = 0.f; // rad
float angle_max = 0.f; // rad
float angle_increment = 0.f; // rad
float time_increment = 0.f; // sec — device doesn't expose per-point timing, always 0
float scan_time = 0.f; // sec — device doesn't expose per-scan timing, always 0
float range_min = 0.f; // m — from ModelConfig, NOT measured per-scan
float range_max = 0.f; // m — from ModelConfig, NOT measured per-scan
float time_increment = 0.f; // sec — not exposed by devices, always 0
float scan_time = 0.f; // sec — not exposed by devices, always 0
float range_min = 0.f; // m — from ModelConfig, not measured
float range_max = 0.f; // m — from ModelConfig, not measured
std::vector<float> ranges; // m
std::vector<float> intensities; // 0-255 read back as float, like ROS does
std::vector<float> intensities; // 0-255 as float
};
// ─── Extra info: whatever diagnostic/header fields THIS family/model exposes ─
// Fields the protocol family doesn't carry stay unset (std::nullopt). Several
// of these are raw, undecoded passthroughs of header bytes whose exact
// meaning hasn't been verified against real hardware/datasheet — see comments
// in olei_lidar.cpp next to where each is read.
// Diagnostic/header fields; fields the family doesn't carry stay std::nullopt.
struct ExtraInfo {
std::string detected_model = "AUTO"; // real model name read from the packet, or "AUTO"
uint8_t error_status = 0; // Family A only; BIT0=Monitor, BIT1=Voltage, BIT2=Temp
uint8_t distance_scale_mm = 0; // mm/count used to decode ranges this scan (0 = not reported)
std::string detected_model = "AUTO";
uint8_t error_status = 0; // Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
uint8_t distance_scale_mm = 0; // 0 = not reported
// Family A (0xFAF0) only — raw 16-bit "rotation info" header field,
// meaning not decoded/verified.
// Family A only
std::optional<uint16_t> rotation_raw;
// Family C / protocol V3 (0xFEAC, GS1-5) only — ported from the C# driver
// header layout, NOT cross-checked against real GS1-5 hardware.
// Family C / V3 (GS1-5) only — raw passthroughs, unverified
std::optional<uint8_t> distance_ratio_raw;
std::optional<uint16_t> scan_frequency_raw;
std::optional<uint16_t> input_status;
@@ -54,144 +40,90 @@ struct ExtraInfo {
std::optional<uint32_t> status_flags;
};
// One complete revolution, in both forms at once.
struct ScanResult {
LaserScan scan;
ExtraInfo info;
};
// ─── Per-model configuration ───────────────────────────────────────────────
// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right.
// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it.
// range_min_m/range_max_m are sensor-spec placeholders (NOT read from any
// packet) used to fill LaserScan::range_min/range_max — adjust to the real
// datasheet values for each model if precision matters to your consumer.
// Per-model configuration. scan_angle_* use the signed system [-180,180]:
// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
struct ModelConfig {
const char* name;
float scan_angle_min; // deg — VB/LR-16F: -135, 360° models: -180
float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180
float scan_angle_min; // deg
float scan_angle_max; // deg
float range_min_m = 0.05f;
float range_max_m = 30.f;
// Added to the raw device angle so output 0° = ahead (LR-1F/1FMI report 0°
// at the back: +180; SICK TiM puts the front at 90°: -90).
float angle_offset_deg = 0.f;
// Output remap window (see make_lidar / remap_scan_window): shifts the
// scan's angles onto [out_angle_min, out_angle_max] without dropping points.
bool remap_angles = false;
float out_angle_min = 0.f; // deg
float out_angle_max = 0.f; // deg
};
// Table of known models — the driver auto-detects the packet family (A=0xFAF0 /
// B=0xFEF0 / C=0xFEAC) per packet, so this config mainly decides the angular
// window (FOV) that gets kept.
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 }; // 2D 360° 50m
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°, 0.01°/LSB ~2400 pts/rev (Family B)
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 }; // 2D 360° (Family B)
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°
// Sentinel: model unknown ahead of time. Family B (0xFEF0) carries an ASCII
// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP
// sniff) → the driver auto-detects it and narrows the FOV per the table
// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has
// no such string, so on a Family-A device MODEL_AUTO keeps the wide default
// FOV (-180..180, no points dropped) until the user specifies a concrete model.
// Model unknown ahead of time: Family B/C packets carry enough to auto-detect;
// Family A doesn't, so the wide default FOV is kept.
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
// callback invoked whenever a complete scan is ready — shared by every driver
// (lidarlib::Driver, lidarlib::SickDriver) and the unified Lidar interface below.
using ScanCallback = std::function<void(const ScanResult&)>;
// ─── Unified driver interface ───────────────────────────────────────────────
// Common handle returned by lidarlib::make_lidar() (the single config function in
// config.hpp). Both the OLEI Driver (UDP) and the SICK SickDriver (TCP) derive
// from this, so a GUI/app can drive any supported lidar through one type and
// never branch on brand. Every call yields the same ScanResult { LaserScan
// scan; ExtraInfo info; } — output #1 (ROS-shaped LaserScan, identical across
// all models) and output #2 (ExtraInfo, model-specific extra fields).
// Unified driver interface returned by make_lidar(); OLEI and SICK drivers
// both derive from it.
class Lidar {
public:
virtual ~Lidar() = default;
// Open the transport (UDP socket / TCP connection) and start receiving.
virtual bool open() = 0;
// Close the transport.
virtual void close() = 0;
// Block until one full scan is received; false on error/timeout.
// timeout_ms = 0 → block indefinitely. (No default here on purpose: the
// concrete drivers differ — OLEI 1000 ms, SICK 2000 ms — so callers using
// the interface must state the timeout they want.)
// Block until one full scan; false on error/timeout. timeout_ms = 0 → block
// indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000).
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
// Or set a callback and drive it from your own loop via spin_once().
virtual void set_scan_callback(ScanCallback cb) = 0;
// Receive + dispatch the callback once (non-owning loop step).
virtual bool spin_once() = 0;
// Real model name read from the packet, or the configured name if the
// family carries none. See Driver::detected_model() for OLEI specifics.
virtual const char* detected_model() const = 0;
};
// ─── Driver ─────────────────────────────────────────────────────────────────
// OLEI UDP driver.
class Driver : public Lidar {
public:
// callback invoked whenever a complete scan is ready
using ScanCallback = lidarlib::ScanCallback;
// ip : receiving host's bind address, usually "0.0.0.0"
// port : UDP port the lidar sends to (default 2368)
// cfg : model config
// inverted : set true if this physical unit is mounted upside-down
// (flipped 180° about its forward-facing axis). Mirrors every
// point's angle (angle = -angle) so output stays in the
// vehicle's frame regardless of mounting orientation — useful
// when e.g. front is mounted normally but rear is flipped.
// ip: local bind address; port: UDP port the lidar sends to;
// inverted: unit mounted upside-down → mirror every angle.
explicit Driver(const ModelConfig& cfg,
const std::string& ip = "0.0.0.0",
uint16_t port = 2368,
bool inverted = false);
~Driver();
// Non-copyable
Driver(const Driver&) = delete;
Driver& operator=(const Driver&) = delete;
// Open the socket and start receiving
bool open() override;
// Close the socket
void close() override;
// Blocks until a full revolution has been received; returns false on error/timeout
// timeout_ms = 0 → block indefinitely
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
// Or use the callback (drive it from your own non-blocking loop)
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
// Receive + dispatch callback (call from your own loop)
bool spin_once() override;
// The REAL model name read from the Family B/C header (only meaningful
// when the Driver was constructed with MODEL_AUTO). Always the actual
// string found in the packet (e.g. "OLELR-1BS2"), even when that model
// has no specific FOV entry in the table (FOV then stays at the 360°
// default). Returns "AUTO" if no Family B/C packet has been seen yet.
// Mirrored per-scan in ScanResult::info::detected_model.
// 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(); }
private:
// ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ──
bool parse_family_a(const uint8_t* buf, int len);
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)
// ── parse Family B packet (ID=0xFEF0): 40B header, 8B block ──
bool parse_family_b(const uint8_t* buf, int len);
// ── parse Family C / protocol V3 packet (Magic=0xFEAC, GS1-5): 48B header ──
bool parse_family_c(const uint8_t* buf, int len);
// Appends one point's angle (already signed+inverted+FOV-filtered by the
// caller), unwrapping it against the previous point in this revolution so
// the accumulated sequence stays continuous across the ±180° seam instead
// of jumping — required for LaserScan::angle_min/angle_max/ranges to stay
// monotonic for 360° devices.
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
// Once a full revolution is ready → flush into ready_result_ and fire the callback
void flush_scan();
ModelConfig cfg_;
@@ -201,27 +133,22 @@ private:
int sock_fd_ = -1;
ScanCallback cb_;
// Per-revolution accumulation buffers (parallel arrays, index-aligned)
std::vector<float> pending_angle_deg_; // unwrapped, continuous
// 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-around (revolution-boundary) detection, device space [0,360)
float last_angle_ = -1.f; // wrap detection, device space [0,360)
// Per-revolution ExtraInfo accumulation — overwritten as packets for the
// in-progress revolution are parsed, then copied into ready_result_ on flush.
ExtraInfo pending_info_;
// recv_scan()'s output, gated by a simple ready flag
ScanResult ready_result_;
bool scan_ready_ = false;
// Per-instance receive buffer — NOT static, so that 2 lidars running on 2
// threads don't overwrite each other's data (data race).
// Per-instance so two drivers on two threads don't race.
uint8_t recv_buf_[4096];
// Model auto-detection from the Family B/C header (see MODEL_AUTO)
bool auto_detect_ = false;
bool model_locked_ = false;
std::string detected_model_name_ = "AUTO";