#pragma once #include #include #include #include #include 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). struct LaserScan { uint32_t timestamp_ms = 0; // device clock (ms since power-on); 0 if the // family doesn't expose one (see ExtraInfo) 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 std::vector ranges; // m std::vector intensities; // 0-255 read back as float, like ROS does }; // ─── 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. 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) // Family A (0xFAF0) only — raw 16-bit "rotation info" header field, // meaning not decoded/verified. std::optional 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. std::optional distance_ratio_raw; std::optional scan_frequency_raw; std::optional input_status; std::optional output_status; std::optional field_status; std::optional 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. 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 range_min_m = 0.05f; float range_max_m = 30.f; }; // 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_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. 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; // ─── 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). 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.) 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 ───────────────────────────────────────────────────────────────── 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. 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. 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); // ── 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_; std::string ip_; uint16_t port_; bool inverted_ = false; int sock_fd_ = -1; ScanCallback cb_; // Per-revolution accumulation buffers (parallel arrays, index-aligned) std::vector pending_angle_deg_; // unwrapped, continuous std::vector pending_dist_m_; std::vector 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) // 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). 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"; }; } // namespace lidarlib