DriverLIdar/include/lidar_interface.hpp

#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 <chrono>
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <vector>

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.
//
// ANGLE CONVENTION CONTRACT (ROS REP-103, right-handed): 0 = device front,
// positive = LEFT, increasing counter-clockwise viewed from the top. Every
// driver must NORMALIZE its device's native scale into this frame — three
// independent, stackable device quirks to absorb:
//   1. shifted 0° reference        -> ModelConfig::angle_offset_deg
//      (OLEI LR-1F/1FMI/1BS5: 0° at the rear; SICK TiM: front at 90°);
//   2. CLOCKWISE angle scale       -> mirror the raw angle (θ -> −θ) BEFORE
//      the offset (all Slamtec RPLIDARs per the Interface Protocol spec;
//      OLEI LR-1BS5, field-verified). Decoding a CW scale as CCW mirrors the
//      whole world left/right — undetectable with a single lidar (the SLAM
//      map is self-consistently mirrored and heading checks still pass), so
//      verify handedness against the real room, not just against motion;
//   3. upside-down mounting        -> the physical flip reverses the
//      apparent rotation, one more angle negation (the driver's `inverted`).
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<float> ranges;       // m
    std::vector<float> 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<uint16_t> rotation_raw;

    // OLEI 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;
    std::optional<uint16_t> output_status;
    std::optional<uint32_t> field_status;
    std::optional<uint32_t> status_flags;

    // SICK TiM only — LMDscandata status pair (word0<<8)|word1:
    // 0 ok, 1 error, 2 pollution warning, 4 pollution error.
    std::optional<uint16_t> sick_device_status;

    // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
    std::optional<uint8_t> 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<uint16_t> 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<uint8_t>  rplidar_health_status;
    std::optional<uint16_t> 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<uint8_t>(~(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<void(const ScanResult&)>;

// ── 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> 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<std::string> 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> 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<std::string, std::string> 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<std::chrono::milliseconds>(
                deadline - std::chrono::steady_clock::now()).count();
            if (left <= 0) return false;
            recv_scan(tmp, static_cast<int>(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"