#pragma once // xlidar-driver — public driver interface. // // Every lidar driver plugin implements xlidar::LidarDriverInterface and // exports two extern "C" entry points (see "Plugin ABI" at the bottom): // // get_driver_info(xlidar::DriverInfo*) — static metadata // create_driver_instance(const xlidar::DeviceConfig*) // — new driver instance // // Host applications never include plugin headers; they talk to plugins // exclusively through this header + lidar_manager.hpp. #include "lidar_diagnostics.hpp" #include #include #include #include #include #include #include #include namespace xlidar { // ── Error codes ───────────────────────────────────────────────────────────── // Result of open() and the sticky status behind last_error(). Ok == 0 so // `if (err != ErrorCode::Ok)` reads naturally at call sites. enum class ErrorCode { Ok = 0, // Lifecycle misuse — the call was refused, the instance state is unchanged. AlreadyOpen, // open() called while already open NotOpen, // recv_scan()/spin_once() called before open() // open() failures SocketError, // socket() creation failed InvalidAddress, // ip string is not a valid IPv4 address PortInUse, // bind: local port already taken (EADDRINUSE/EACCES) BindFailed, // bind failed for another reason ConnectionRefused, // TCP connect refused (device up, port closed) ConnectionFailed, // TCP connect failed (unreachable, no route, ...) HandshakeFailed, // connected, but the start-stream command failed SerialError, // serial port open/configure failed (serial drivers) DeviceError, // device rejected a command / reported a hard fault // Runtime failures Timeout, // no (complete) scan within timeout_ms DeviceDisconnected, // peer closed the connection / socket or serial error // Configuration errors InvalidConfig, // DeviceConfig is not usable by this driver }; inline const char* to_string(ErrorCode e) { switch (e) { case ErrorCode::Ok: return "Ok"; case ErrorCode::AlreadyOpen: return "AlreadyOpen"; case ErrorCode::NotOpen: return "NotOpen"; case ErrorCode::SocketError: return "SocketError"; case ErrorCode::InvalidAddress: return "InvalidAddress"; case ErrorCode::PortInUse: return "PortInUse"; case ErrorCode::BindFailed: return "BindFailed"; case ErrorCode::ConnectionRefused: return "ConnectionRefused"; case ErrorCode::ConnectionFailed: return "ConnectionFailed"; case ErrorCode::HandshakeFailed: return "HandshakeFailed"; case ErrorCode::SerialError: return "SerialError"; case ErrorCode::DeviceError: return "DeviceError"; case ErrorCode::Timeout: return "Timeout"; case ErrorCode::DeviceDisconnected: return "DeviceDisconnected"; case ErrorCode::InvalidConfig: return "InvalidConfig"; } return "Unknown"; } // ── Scan data ─────────────────────────────────────────────────────────────── // 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 most devices, 0 then float scan_time = 0.f; // sec — not exposed by most devices, 0 then 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 device family doesn't carry stay // std::nullopt (see docs/diagnostics.md for the per-family wire layout). struct ExtraInfo { std::string detected_model = "AUTO"; uint8_t error_status = 0; // OLEI Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp uint8_t distance_scale_mm = 0; // 0 = not reported // OLEI Family A only std::optional rotation_raw; // OLEI 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; // SICK TiM only — LMDscandata status pair (word0<<8)|word1: // 0 ok, 1 error, 2 pollution warning, 4 pollution error. std::optional sick_device_status; // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits). std::optional nano_general_state; // ESPE LGA60 only — fault word from the newest "WSimu" area frame; the // device only sends those when area data is polled, so usually nullopt. std::optional espe_error_status; // RPLIDAR only — SDK health status (0 ok, 1 warning, 2 error) and the // device error code that goes with it. std::optional rplidar_health_status; std::optional rplidar_error_code; }; struct ScanResult { LaserScan scan; ExtraInfo info; }; // Decode the diagnostic fields of one scan into the vendor-neutral // Diagnostics structure; sets valid = true. Vendor bit layouts are decoded // here (constants in lidar_diagnostics.hpp) so hosts only ever see common // issue codes; the raw values ride along in Diagnostics::raw. inline Diagnostics decode_diagnostics(const ExtraInfo& info) { Diagnostics d; d.valid = true; d.model = info.detected_model; const auto add = [&d](DiagSeverity severity, const char* code, std::string detail) { d.issues.push_back({severity, code, std::move(detail)}); }; char buf[48]; // OLEI Family A error byte (Family B/C don't carry it — stays 0). if (info.error_status != 0) { d.raw["olei.error_status"] = info.error_status; if (info.error_status & kFaultMonitor) add(DiagSeverity::Fault, "motor", "OLEI monitor/motor abnormal"); if (info.error_status & kFaultVoltage) add(DiagSeverity::Fault, "voltage", "OLEI supply voltage out of range"); if (info.error_status & kFaultTemperature) add(DiagSeverity::Fault, "temperature", "OLEI internal temperature abnormal"); if (const uint8_t rest = info.error_status & static_cast(~(kFaultMonitor | kFaultVoltage | kFaultTemperature))) { std::snprintf(buf, sizeof(buf), "OLEI reserved error bits 0x%02X", rest); add(DiagSeverity::Fault, "device_error", buf); } } // OLEI raw passthroughs (meanings unverified — no issue decoding). if (info.rotation_raw) d.raw["olei.rotation"] = *info.rotation_raw; if (info.distance_ratio_raw) d.raw["olei.distance_ratio"] = *info.distance_ratio_raw; if (info.scan_frequency_raw) d.raw["olei.scan_frequency"] = *info.scan_frequency_raw; if (info.input_status) d.raw["olei.input_status"] = *info.input_status; if (info.output_status) d.raw["olei.output_status"] = *info.output_status; if (info.field_status) d.raw["olei.field_status"] = *info.field_status; if (info.status_flags) d.raw["olei.status_flags"] = *info.status_flags; // SICK TiM device status pair. if (info.sick_device_status) { d.raw["sick.device_status"] = *info.sick_device_status; if (*info.sick_device_status & kSickStatusError) add(DiagSeverity::Fault, "device_error", "SICK TiM device error"); if (*info.sick_device_status & kSickStatusPollutionWarning) add(DiagSeverity::Warning, "optics_dirty", "SICK TiM pollution warning"); if (*info.sick_device_status & kSickStatusPollutionError) add(DiagSeverity::Fault, "optics_dirty", "SICK TiM pollution error"); } // SICK nanoScan3 general system state. if (info.nano_general_state) { d.raw["nano.general_state"] = *info.nano_general_state; if (*info.nano_general_state & kNanoStateContaminationWarning) add(DiagSeverity::Warning, "optics_dirty", "nanoScan3 contamination warning"); if (*info.nano_general_state & kNanoStateContaminationError) add(DiagSeverity::Fault, "optics_dirty", "nanoScan3 contamination error"); if (*info.nano_general_state & kNanoStateManipulation) add(DiagSeverity::Fault, "manipulation", "nanoScan3 manipulation suspected"); } // ESPE fault word (bit meanings unverified). if (info.espe_error_status) { d.raw["espe.error_status"] = *info.espe_error_status; if (*info.espe_error_status != 0) { std::snprintf(buf, sizeof(buf), "ESPE fault word 0x%04X", *info.espe_error_status); add(DiagSeverity::Fault, "device_error", buf); } } // RPLIDAR SDK health. if (info.rplidar_health_status) { d.raw["rplidar.health_status"] = *info.rplidar_health_status; if (info.rplidar_error_code) d.raw["rplidar.error_code"] = *info.rplidar_error_code; if (*info.rplidar_health_status == kRplidarHealthError) { std::snprintf(buf, sizeof(buf), "RPLIDAR health error, code 0x%04X", info.rplidar_error_code ? *info.rplidar_error_code : 0); add(DiagSeverity::Fault, "device_error", buf); } else if (*info.rplidar_health_status == kRplidarHealthWarning) { add(DiagSeverity::Warning, "device_warning", "RPLIDAR health warning"); } } return d; } // Per-model configuration. scan_angle_* use the signed system [-180,180]: // 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders. // The per-vendor MODEL_* presets live in each plugin. 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 (e.g. 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 remap_scan_window in plugins/common): 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 // Valid FOV window from DeviceConfig::angle_min/max_deg: points outside // become NaN, geometry unchanged (apply_fov_window in plugins/common). bool fov_filter = false; float fov_min_deg = -360.f; float fov_max_deg = 360.f; }; using ScanCallback = std::function; // ── Driver metadata & instance configuration ──────────────────────────────── // Transport a driver uses to reach the device. A driver declares exactly one // primary transport; drivers that can switch (e.g. ESPE TCP/UDP) declare the // default and honor DeviceConfig::transport. enum class Transport { Serial, Udp, Tcp }; inline const char* to_string(Transport t) { switch (t) { case Transport::Serial: return "serial"; case Transport::Udp: return "udp"; case Transport::Tcp: return "tcp"; } return "unknown"; } // Parse the strings written by to_string(Transport); nullopt for anything else. inline std::optional transport_from_string(const std::string& s) { if (s == "serial") return Transport::Serial; if (s == "udp") return Transport::Udp; if (s == "tcp") return Transport::Tcp; return std::nullopt; } // Static identity a plugin registers about itself (get_driver_info entry // point and LidarDriverInterface::get_driver_info()). struct DriverInfo { std::string vendor; // "Slamtec", "OLEI", "SICK", "ESPE" std::string model; // device category the driver targets, e.g. "C1" // or "TiM5xx/TiM7xx" — one driver may cover a // whole series std::string driver_id; // unique stable id, e.g. "rplidar_c1_driver" std::string description; // short doc: covered devices, transport, notes // Extra metadata for hosts/UIs (not part of the required identity): Transport transport = Transport::Udp; // primary transport bool transport_selectable = false; // true → DeviceConfig::transport // may pick either TCP or UDP std::vector supported_models; // valid DeviceConfig::model values }; // Settings for one lidar instance. `name` is the unique key across saves. // Which fields matter depends on the transport in effect: // serial → serial_port + baudrate; udp/tcp → ip + port. struct DeviceConfig { std::string name = "lidar"; std::string driver_id; // plugin that owns this device std::string model = "AUTO"; // one of DriverInfo::supported_models // Transport to reach the device. nullopt = the driver's declared default // (DriverInfo::transport). A fixed-transport driver rejects a mismatch // from open() with InvalidConfig; transport-selectable drivers (ESPE) // switch between TCP and UDP through this field. std::optional transport; // Network transports (udp: local bind address / tcp: device address) std::string ip = "0.0.0.0"; uint16_t port = 0; // 0 = driver default // Serial transport std::string serial_port = "/dev/ttyUSB0"; uint32_t baudrate = 460800; bool inverted = false; // unit mounted upside-down → mirror the scan // Valid field-of-view window (deg, signed system: 0 = ahead, + = left). // Points outside are reported as NaN (invalid), the scan geometry is // unchanged. Defaults (±360) = off. float angle_min_deg = -360.f; float angle_max_deg = 360.f; // Range override (m); 0 = keep the driver/model default. float range_min_m = 0.f; float range_max_m = 0.f; // Legacy output remap window (deg): scan angles are linearly remapped // onto [remap_angle_min_deg, remap_angle_max_deg] without dropping // points. Defaults (±360) = off. Kept for pre-plugin lidarlib configs. float remap_angle_min_deg = -360.f; float remap_angle_max_deg = 360.f; // Driver-specific options that don't warrant a first-class field // (documented per plugin). std::map extra; friend bool operator==(const DeviceConfig& a, const DeviceConfig& b) { return a.name == b.name && a.driver_id == b.driver_id && a.model == b.model && a.transport == b.transport && a.ip == b.ip && a.port == b.port && a.serial_port == b.serial_port && a.baudrate == b.baudrate && a.inverted == b.inverted && a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg && a.range_min_m == b.range_min_m && a.range_max_m == b.range_max_m && a.remap_angle_min_deg == b.remap_angle_min_deg && a.remap_angle_max_deg == b.remap_angle_max_deg && a.extra == b.extra; } friend bool operator!=(const DeviceConfig& a, const DeviceConfig& b) { return !(a == b); } }; // ── Driver interface ──────────────────────────────────────────────────────── // Unified driver interface implemented by every plugin. Instances come from // LidarManager::create_lidar_device() (or a plugin's create_driver_instance // entry point directly). One instance == one physical device; instances are // fully independent — run each on its own thread without locking. class LidarDriverInterface { public: virtual ~LidarDriverInterface() = default; // Static metadata of the driver that produced this instance. virtual DriverInfo get_driver_info() const = 0; // 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 TiM 2000). virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; // The callback fires only from spin_once() — recv_scan() never invokes // it. Pick one pump style: recv_scan() to poll, or callback + spin_once(). virtual void set_scan_callback(ScanCallback cb) = 0; // Process one unit of input (may block on the socket/port while the // device is silent); fires the scan callback when a scan completed. // False on error. virtual bool spin_once() = 0; // Model name read from the wire where the protocol carries one // ("AUTO"/configured name until then). 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_; } // Device self-diagnostics from the newest fully decoded scan. valid stays // false until one scan has been seen. Updated by recv_scan()/spin_once(); // call from the same thread that pumps them. virtual Diagnostics get_diagnostics() const { return {}; } // True when the sensor is usable right now: connection open, at least one // fault-free scan decoded, and that scan no older than max_age_ms // (0 = skip the age check). Diagnostics only refresh from // recv_scan()/spin_once(), so unless something is pumping them this goes // stale and reports not-ready; call from the pump thread. bool is_ready(int max_age_ms = 3000) const { if (!is_open() || !get_diagnostics().healthy()) return false; if (max_age_ms <= 0) return true; return last_scan_time_.time_since_epoch().count() != 0 && std::chrono::steady_clock::now() - last_scan_time_ <= std::chrono::milliseconds(max_age_ms); } // Pump recv_scan() until is_ready() or timeout_ms elapses; false on // timeout (see last_error() for the underlying failure). Scans consumed // while waiting are discarded and the scan callback does not fire — // intended for startup, before handing the pump to the main loop. bool wait_ready(int timeout_ms = 5000) { const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); ScanResult tmp; while (!is_ready()) { if (!is_open()) return false; const auto left = std::chrono::duration_cast( deadline - std::chrono::steady_clock::now()).count(); if (left <= 0) return false; recv_scan(tmp, static_cast(left)); } return true; } protected: ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; } // Drivers call this each time a full scan is decoded; feeds the freshness // side of is_ready(). void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); } private: ErrorCode last_error_ = ErrorCode::Ok; std::chrono::steady_clock::time_point last_scan_time_{}; }; } // namespace xlidar // ── Plugin ABI ────────────────────────────────────────────────────────────── // // Each plugin .so exports exactly these two symbols (C linkage, default // visibility — plugins are otherwise built with -fvisibility=hidden): // // XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out); // XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface* // create_driver_instance(const xlidar::DeviceConfig* cfg); // // create_driver_instance returns a heap-allocated instance (never nullptr for // a structurally valid cfg; config problems surface from open() as // InvalidConfig/SerialError/...). The host deletes it through the // LidarDriverInterface vtable, so the plugin must stay loaded for the // instance's whole lifetime — LidarManager guarantees that by keeping every // plugin open until the manager itself is destroyed. // // C linkage keeps the symbol names unmangled for dlsym(); the types crossing // the boundary are C++ (same toolchain for manager and plugins is required — // they are always built together in this repo). #define XLIDAR_PLUGIN_EXPORT extern "C" __attribute__((visibility("default"))) extern "C" { using xlidar_get_driver_info_fn = void (*)(xlidar::DriverInfo*); using xlidar_create_driver_instance_fn = xlidar::LidarDriverInterface* (*)(const xlidar::DeviceConfig*); } // Symbol names LidarManager resolves in every plugin. #define XLIDAR_GET_DRIVER_INFO_SYMBOL "get_driver_info" #define XLIDAR_CREATE_DRIVER_INSTANCE_SYMBOL "create_driver_instance"