DriverLIdar/plugins/driver_espe/espe_driver.cpp

// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP.
#include "espe_driver.hpp"
#include "plugin_helpers.hpp"

#include <algorithm>
#include <cerrno>
#include <cmath>
#include <cstring>
#include <limits>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <unistd.h>

namespace xlidar {

namespace {
// "RAuto" + fixed tail — puts the device into continuous measurement output.
constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};

constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
constexpr char kAreaMagic[5]  = {'W', 'S', 'i', 'm', 'u'};

constexpr size_t   kRangeHeaderSize = 16;    // magic + 6 big-endian u16 fields
constexpr size_t   kAreaFrameSize   = 13;    // magic + 4 status bytes + err u16 + crc u16
constexpr uint16_t kMaxDistanceMm   = 50000; // wire sentinel: beyond = no return
constexpr uint16_t kMaxIntensity    = 30000;
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
constexpr int      kConnectTimeoutMs = 2000;

uint16_t be16(const uint8_t* p) {
    return static_cast<uint16_t>((p[0] << 8) | p[1]);
}
} // namespace

EspeDriver::EspeDriver(const ModelConfig& cfg, const std::string& ip,
                       uint16_t port, bool use_udp, bool inverted)
    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip),
      port_(port), use_udp_(use_udp), inverted_(inverted) {}

EspeDriver::~EspeDriver() { close(); }

ErrorCode EspeDriver::open() {
    if (is_open()) return set_error(ErrorCode::AlreadyOpen);

    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_port   = htons(port_);
    if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
        return set_error(ErrorCode::InvalidAddress);

    sock_fd_ = ::socket(AF_INET, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);

    ErrorCode conn_err = ErrorCode::Ok;
    if (use_udp_) {
        // connect() on UDP just fixes the peer; replies come to our port.
        if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
            conn_err = ErrorCode::ConnectionFailed;
    } else {
        conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
    }

    if (conn_err != ErrorCode::Ok) {
        ::close(sock_fd_);
        sock_fd_ = -1;
        return set_error(conn_err);
    }

    recv_buf_.clear();
    points_total_ = 0;
    pending_time_ = 0;
    scan_ready_   = false;
    espe_error_status_.reset();
    latest_diag_  = Diagnostics{};

    // Device is passive until told to stream.
    ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
    if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
        close();
        return set_error(ErrorCode::HandshakeFailed);
    }
    return set_error(ErrorCode::Ok);
}

void EspeDriver::close() {
    if (sock_fd_ >= 0) {
        ::close(sock_fd_);
        sock_fd_ = -1;
    }
}

bool EspeDriver::fill_buffer(int timeout_ms) {
    if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }

    if (timeout_ms > 0) {
        fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
        int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
        if (r <= 0) {
            set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
            return false;
        }
    }

    char buf[4096];
    ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
    if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
    recv_buf_.append(buf, static_cast<size_t>(n));
    return true;
}

// Consume complete frames from recv_buf_; returns true once a full revolution
// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
bool EspeDriver::parse_buffer() {
    for (;;) {
        size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
        size_t area_pos  = recv_buf_.find(kAreaMagic,  0, sizeof(kAreaMagic));
        size_t pos = std::min(range_pos, area_pos);
        if (pos == std::string::npos) {
            // No magic in sight: keep only a possible partial magic at the tail.
            if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
                recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
            return scan_ready_;
        }
        if (pos > 0) recv_buf_.erase(0, pos);

        const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());

        if (area_pos < range_pos) {
            if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
            // Zone/obstacle frame — only sent when the host polls areas, but
            // it carries the device fault word, so latch it if it appears.
            // Byte order unverified on hardware: the protocol is mixed-endian
            // (header fields big-endian, point payload little-endian) and no
            // spec covers this field; little-endian assumed like the payload.
            espe_error_status_ = le16(d + 9);
            recv_buf_.erase(0, kAreaFrameSize);
            continue;
        }

        if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
        uint16_t data_size    = be16(d + 8);
        uint16_t measure_size = be16(d + 12);
        if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
            recv_buf_.erase(0, sizeof(kRangeMagic));  // bogus header — resync
            continue;
        }
        if (data_size > measure_size) data_size = measure_size;

        size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
        if (recv_buf_.size() < frame_size) return scan_ready_;

        handle_range_frame(d, data_size);
        recv_buf_.erase(0, frame_size);
        // Stop as soon as a revolution completes — draining further frames
        // could finish a second revolution and overwrite ready_result_ before
        // the caller consumes it. Leftover bytes wait for the next call.
        if (scan_ready_) return true;
    }
}

// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
// data_size (points in this frame), data_position (cumulative points incl.
// this frame), measure_size (points per revolution), time; then data_size ×
// 4 B little-endian (u16 distance mm, u16 intensity).
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
    uint16_t start_angle   = be16(frame + 4);
    uint16_t end_angle     = be16(frame + 6);
    uint16_t data_position = be16(frame + 10);
    uint16_t measure_size  = be16(frame + 12);
    pending_time_          = be16(frame + 14);

    // First frame of a revolution (or geometry changed) → start a new one.
    if (points_total_ != measure_size || data_position <= data_size) {
        points_total_   = measure_size;
        rev_start_deg_  = static_cast<float>(start_angle);
        angle_inc_deg_  = static_cast<float>(end_angle - start_angle) / measure_size;
        pending_ranges_.assign(points_total_, 0.f);
        pending_intensities_.assign(points_total_, 0.f);
    }
    if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }

    // start_angle is normally constant across the revolution, so this is just
    // the cumulative position; the angle term covers firmware that advances it.
    int32_t begin = static_cast<int32_t>(std::lround(
                        (static_cast<float>(start_angle) - rev_start_deg_) / angle_inc_deg_))
                  + static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);

    const uint8_t* p = frame + kRangeHeaderSize;
    for (uint16_t i = 0; i < data_size; ++i, p += 4) {
        int32_t idx = begin + i;
        if (idx < 0 || idx >= static_cast<int32_t>(points_total_)) continue;
        uint16_t dist  = le16(p + 0);
        uint16_t inten = le16(p + 2);
        pending_ranges_[idx] = (dist > kMaxDistanceMm)
            ? std::numeric_limits<float>::infinity()
            : static_cast<float>(dist) * 1e-3f;  // mm -> m
        // Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
        pending_intensities_[idx] =
            static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
            * (255.f / kMaxIntensity);
    }

    if (data_position >= points_total_) finish_scan();
}

void EspeDriver::finish_scan() {
    LaserScan& scan = ready_result_.scan;
    scan = LaserScan{};
    scan.timestamp_ms = pending_time_;  // header "time" field, unit unverified
    scan.ranges      = std::move(pending_ranges_);
    scan.intensities = std::move(pending_intensities_);
    scan.angle_min       = (rev_start_deg_ + cfg_.angle_offset_deg) * kDeg2Rad;
    scan.angle_increment = angle_inc_deg_ * kDeg2Rad;
    scan.angle_max       = scan.angle_min +
        scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
    scan.range_min = cfg_.range_min_m;
    scan.range_max = cfg_.range_max_m;

    finalize_scan(scan, cfg_, inverted_);

    ExtraInfo& info = ready_result_.info;
    info = ExtraInfo{};
    info.detected_model    = cfg_.name;
    info.espe_error_status = espe_error_status_;

    latest_diag_ = decode_diagnostics(info);
    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
    mark_scan_decoded();

    pending_ranges_.clear();
    pending_intensities_.clear();
    points_total_ = 0;
    scan_ready_   = true;
}

bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
    for (;;) {
        if (parse_buffer()) {
            scan_ready_ = false;
            out = std::move(ready_result_);
            set_error(ErrorCode::Ok);
            return true;
        }
        if (!fill_buffer(timeout_ms)) return false;
    }
}

bool EspeDriver::spin_once() {
    if (!parse_buffer()) {
        if (!fill_buffer(0)) return false;
        parse_buffer();
    }
    if (scan_ready_) {
        scan_ready_ = false;
        if (cb_) cb_(ready_result_);
    }
    return true;
}

// ── plugin registration ─────────────────────────────────────────────────────

namespace {

const DriverInfo kDriverInfo = [] {
    DriverInfo info;
    info.vendor      = "ESPE";
    info.model       = "LGA60";
    info.driver_id   = "espe_lga60_driver";
    info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
                       "use_udp; open() sends the RAuto start command; device "
                       "parameters come from the vendor Windows tool. Default "
                       "port 8080 (vendor default IP 192.168.1.88). Ported from "
                       "the vendor ROS driver; not verified on real hardware.";
    info.transport   = Transport::Tcp;
    info.transport_selectable = true;   // use_udp switches to UDP
    info.supported_models = {"ESPE-LGA60"};
    return info;
}();

} // namespace

DriverInfo EspeDriver::get_driver_info() const { return kDriverInfo; }

} // namespace xlidar

XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
    *out = xlidar::kDriverInfo;
}

XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
create_driver_instance(const xlidar::DeviceConfig* cfg) {
    using namespace xlidar;
    const uint16_t port = cfg->port ? cfg->port : 8080;
    return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
                          cfg->ip, port, cfg->use_udp, cfg->inverted);
}