#pragma once #include "lidarlib/error.hpp" #include #include #include #include #include namespace lidarlib { // 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); 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 — 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 ranges; // m std::vector intensities; // 0-255 as float }; // Diagnostic/header fields; fields the family doesn't carry stay std::nullopt. struct ExtraInfo { 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 only std::optional rotation_raw; // Family C / V3 (GS1-5) only — raw passthroughs, unverified 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; }; struct ScanResult { LaserScan scan; ExtraInfo info; }; // 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 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 }; 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 }; // 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° // 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 }; using ScanCallback = std::function; // Unified driver interface returned by make_lidar(); OLEI and SICK drivers // both derive from it. class Lidar { public: virtual ~Lidar() = default; // ErrorCode::Ok on success. Calling open() on an already-open instance // returns AlreadyOpen and leaves the connection untouched. virtual ErrorCode open() = 0; // Idempotent: safe to call before open() or more than once. virtual void close() = 0; // Block until one full scan; false on error/timeout (see last_error()). // 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; virtual void set_scan_callback(ScanCallback cb) = 0; virtual bool spin_once() = 0; virtual const char* detected_model() const = 0; virtual bool is_open() const = 0; // Status of the most recent open()/recv_scan()/spin_once() call. ErrorCode last_error() const { return last_error_; } protected: ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; } private: ErrorCode last_error_ = ErrorCode::Ok; }; // OLEI UDP driver. class Driver : public Lidar { public: using ScanCallback = lidarlib::ScanCallback; // 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(); Driver(const Driver&) = delete; Driver& operator=(const Driver&) = delete; 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(); } private: 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(); ModelConfig cfg_; std::string ip_; uint16_t port_; bool inverted_ = false; int sock_fd_ = -1; ScanCallback cb_; // Per-revolution accumulation buffers (index-aligned) std::vector pending_angle_deg_; 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 detection, device space [0,360) ExtraInfo pending_info_; 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 lidarlib