DriverLIdar/plugins/driver_rplidar/rplidar_driver.cpp

// Slamtec RPLIDAR over serial (C1 defaults), built on the vendor SDK
// (sl_lidar.h). Ported from xlocd's embedded rplidar driver — the scan math
// (angle/distance decoding, inversion, FOV window, NaN invalid points) is
// kept identical.
//
// Unlike the network drivers, angles are reported in the DEVICE frame
// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData
// yields (and what xlocd's engine expects).
#include "lidar_interface.hpp"
#include "plugin_helpers.hpp"

#include <chrono>
#include <cmath>
#include <cstdio>
#include <filesystem>
#include <limits>
#include <string>
#include <vector>

#include "sl_lidar.h"

namespace xlidar {

namespace {

// Node buffer for one grab. 8192 is the SDK-recommended size, far above the
// ~400-500 points/rev of a C1 in DenseBoost mode.
constexpr std::size_t kMaxNodesPerScan = 8192;
constexpr float kPi = 3.14159265358979323846F;
constexpr float kTwoPi = 2.0F * kPi;
// C1 default range (datasheet: 12 m on white; 16 m ceiling matches the
// common rplidar_ros configuration). Overridable via DeviceConfig range_*.
constexpr float kDefaultRangeMinM = 0.05F;
constexpr float kDefaultRangeMaxM = 16.0F;
// Nominal rotation period (~10 Hz) for the first frame, before a real
// grab-to-grab interval has been measured.
constexpr float kDefaultScanTimeS = 0.1F;
constexpr int   kDefaultGrabTimeoutMs = 2000;  // SDK default

inline constexpr ModelConfig MODEL_RPLIDAR_C1 { "C1", -180.f, 180.f, kDefaultRangeMinM, kDefaultRangeMaxM };

// HQ node angle: angle_z_q14 is [0..360) fixed-point Q14 on a 90° scale.
float node_angle_rad(const sl_lidar_response_measurement_node_hq_t& node) {
    return static_cast<float>(node.angle_z_q14) * 90.0F / (1 << 14) * kDeg2Rad;
}

// HQ node distance: dist_mm_q2 is mm in Q2 (1/4 mm) -> metres.
float node_distance_m(const sl_lidar_response_measurement_node_hq_t& node) {
    return static_cast<float>(node.dist_mm_q2) / 4.0F / 1000.0F;
}

// Device angle [0, 2π) -> signed (-180, 180] degrees (0 = ahead, + = left),
// to compare against the configured FOV window.
float to_signed_deg(float angle_rad) {
    float deg = angle_rad / kDeg2Rad;
    if (deg > 180.0F) deg -= 360.0F;
    return deg;
}

const DriverInfo kDriverInfo = [] {
    DriverInfo info;
    info.vendor      = "Slamtec";
    info.model       = "C1";
    info.driver_id   = "rplidar_c1_driver";
    info.description = "Slamtec RPLIDAR over serial, built on the vendor SDK — "
                       "defaults match the C1 (CP2102N UART bridge, baud "
                       "460800); other SDK-compatible serial models (A/S "
                       "series) work with the matching baud rate. Health check "
                       "at open(); model/firmware auto-detected.";
    info.transport   = Transport::Serial;
    info.supported_models = {"AUTO", "C1"};
    return info;
}();

} // namespace

class RplidarDriver : public LidarDriverInterface {
public:
    RplidarDriver(const ModelConfig& cfg, std::string serial_port, uint32_t baudrate,
                  bool inverted)
        : cfg_(cfg), serial_port_(std::move(serial_port)), baudrate_(baudrate),
          inverted_(inverted) {}

    ~RplidarDriver() override { close(); }

    RplidarDriver(const RplidarDriver&)            = delete;
    RplidarDriver& operator=(const RplidarDriver&) = delete;

    DriverInfo get_driver_info() const override { return kDriverInfo; }

    // Full connect sequence; each step maps to one ErrorCode:
    // device present (SerialError) -> serial channel (SerialError) -> SDK
    // driver (SerialError) -> connect (ConnectionFailed) -> device info
    // (non-fatal, fills model/firmware) -> health check (DeviceError on
    // fault) -> motor + startScan typical mode (HandshakeFailed).
    ErrorCode open() override {
        if (is_open()) return set_error(ErrorCode::AlreadyOpen);

        std::error_code fs_error;
        if (!std::filesystem::exists(serial_port_, fs_error))
            return set_error(ErrorCode::SerialError);

        auto channel = sl::createSerialPortChannel(serial_port_, static_cast<int>(baudrate_));
        if (!channel) return set_error(ErrorCode::SerialError);
        channel_ = *channel;

        auto lidar = sl::createLidarDriver();
        if (!lidar) { disconnect(); return set_error(ErrorCode::SerialError); }
        lidar_ = *lidar;

        if (!SL_IS_OK(lidar_->connect(channel_))) {
            disconnect();
            return set_error(ErrorCode::ConnectionFailed);
        }

        // Identification — failure here is non-fatal (fields stay empty).
        detected_model_name_ = cfg_.name;
        firmware_.clear();
        sl_lidar_response_device_info_t info{};
        if (SL_IS_OK(lidar_->getDeviceInfo(info))) {
            char model_buf[32];
            std::snprintf(model_buf, sizeof(model_buf), "slamtec-0x%02X",
                          static_cast<unsigned>(info.model));
            char firmware_buf[48];
            std::snprintf(firmware_buf, sizeof(firmware_buf), "fw %u.%02u hw %u",
                          static_cast<unsigned>(info.firmware_version >> 8),
                          static_cast<unsigned>(info.firmware_version & 0xFF),
                          static_cast<unsigned>(info.hardware_version));
            detected_model_name_ = model_buf;
            firmware_            = firmware_buf;
        }

        // Mandatory health check: a self-reported Fault means the data is not
        // usable; Warning still runs but stays visible in diagnostics.
        sl_lidar_response_device_health_t health{};
        if (!SL_IS_OK(lidar_->getHealth(health)) || health.status == SL_LIDAR_STATUS_ERROR) {
            health_status_ = SL_LIDAR_STATUS_ERROR;
            health_error_code_ = static_cast<uint16_t>(health.error_code);
            refresh_diag_from_health();
            disconnect();
            return set_error(ErrorCode::DeviceError);
        }
        health_status_     = health.status;
        health_error_code_ = static_cast<uint16_t>(health.error_code);
        refresh_diag_from_health();

        // C1 spins the motor on the scan command; setMotorSpeed stays for
        // DTR-controlled models (A-series).
        (void)lidar_->setMotorSpeed();
        sl::LidarScanMode scan_mode{};
        if (!SL_IS_OK(lidar_->startScan(false, true, 0, &scan_mode))) {
            (void)lidar_->setMotorSpeed(0);
            disconnect();
            return set_error(ErrorCode::HandshakeFailed);
        }

        have_last_grab_ = false;
        return set_error(ErrorCode::Ok);
    }

    void close() override {
        if (lidar_ != nullptr) {
            (void)lidar_->stop();
            (void)lidar_->setMotorSpeed(0);
        }
        disconnect();
    }

    // Blocks until the SDK hands over one full revolution.
    bool recv_scan(ScanResult& out, int timeout_ms) override {
        if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }

        std::vector<sl_lidar_response_measurement_node_hq_t> nodes(kMaxNodesPerScan);
        std::size_t count = nodes.size();
        const auto grabbed = lidar_->grabScanDataHq(
            nodes.data(), count,
            timeout_ms > 0 ? static_cast<sl_u32>(timeout_ms) : kDefaultGrabTimeoutMs);
        if (!SL_IS_OK(grabbed) || count < 2) {
            set_error(grabbed == SL_RESULT_OPERATION_TIMEOUT ? ErrorCode::Timeout
                                                             : ErrorCode::DeviceDisconnected);
            return false;
        }
        (void)lidar_->ascendScanData(nodes.data(), count);

        // Real rotation period = interval between consecutive grabs (~86 ms
        // on a C1); the first frame uses the nominal value.
        const auto grab_time = std::chrono::steady_clock::now();
        const float scan_time = have_last_grab_
            ? std::chrono::duration<float>(grab_time - last_grab_).count()
            : kDefaultScanTimeS;
        last_grab_      = grab_time;
        have_last_grab_ = true;

        const float angle_first = node_angle_rad(nodes.front());
        const float angle_last  = node_angle_rad(nodes[count - 1]);
        if (angle_last <= angle_first) {
            set_error(ErrorCode::Timeout);  // malformed revolution — treat as a miss
            return false;
        }

        LaserScan& scan = out.scan;
        scan = LaserScan{};
        // Inverted mount -> mirror the angles (angle' = 2π - angle) and walk
        // the nodes backwards to keep ascending order.
        if (inverted_) {
            scan.angle_min = kTwoPi - angle_last;
            scan.angle_max = kTwoPi - angle_first;
        } else {
            scan.angle_min = angle_first;
            scan.angle_max = angle_last;
        }
        scan.angle_increment = (scan.angle_max - scan.angle_min) / static_cast<float>(count - 1);
        scan.scan_time       = scan_time;
        scan.time_increment  = scan_time / static_cast<float>(count);
        scan.range_min       = cfg_.range_min_m;
        scan.range_max       = cfg_.range_max_m;

        // Valid FOV window — only filter when narrower than the full circle.
        const bool apply_angle_window =
            cfg_.fov_filter && (cfg_.fov_min_deg > -180.0F || cfg_.fov_max_deg < 180.0F);

        scan.ranges.reserve(count);
        scan.intensities.reserve(count);
        for (std::size_t i = 0; i < count; ++i) {
            const std::size_t node_index = inverted_ ? count - 1 - i : i;
            // dist = 0 is the SDK's "no return" sentinel; together with
            // out-of-range / out-of-window points it becomes NaN.
            const float distance = node_distance_m(nodes[node_index]);
            bool valid = nodes[node_index].dist_mm_q2 != 0 &&
                         distance >= scan.range_min && distance <= scan.range_max;
            if (valid && apply_angle_window) {
                const float grid_angle = scan.angle_min + scan.angle_increment * static_cast<float>(i);
                const float signed_deg = to_signed_deg(grid_angle);
                valid = signed_deg >= cfg_.fov_min_deg && signed_deg <= cfg_.fov_max_deg;
            }
            scan.ranges.push_back(valid ? distance : std::numeric_limits<float>::quiet_NaN());
            scan.intensities.push_back(static_cast<float>(nodes[node_index].quality));
        }

        if (cfg_.remap_angles)
            remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);

        ExtraInfo& info = out.info;
        info = ExtraInfo{};
        info.detected_model        = detected_model_name_;
        info.rplidar_health_status = health_status_;
        info.rplidar_error_code    = health_error_code_;

        latest_diag_          = decode_diagnostics(info);
        latest_diag_.firmware = firmware_;
        mark_scan_decoded();

        set_error(ErrorCode::Ok);
        return true;
    }

    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }

    // One unit of input == one revolution for this device.
    bool spin_once() override {
        ScanResult result;
        if (!recv_scan(result, kDefaultGrabTimeoutMs)) return false;
        if (cb_) cb_(result);
        return true;
    }

    bool is_open() const override { return lidar_ != nullptr; }

    const char* detected_model() const override { return detected_model_name_.c_str(); }

    Diagnostics get_diagnostics() const override { return latest_diag_; }

private:
    // Health snapshot -> diagnostics, so a fault is visible before the first
    // scan (valid = true means "health was read", not "a scan was decoded").
    void refresh_diag_from_health() {
        ExtraInfo info;
        info.detected_model        = detected_model_name_;
        info.rplidar_health_status = health_status_;
        info.rplidar_error_code    = health_error_code_;
        latest_diag_          = decode_diagnostics(info);
        latest_diag_.firmware = firmware_;
    }

    // The SDK factories hand out raw pointers and require the caller to
    // delete them (see sl_lidar_driver.h) — this is the only place doing so.
    void disconnect() {
        if (lidar_ != nullptr) { delete lidar_; lidar_ = nullptr; }
        if (channel_ != nullptr) { delete channel_; channel_ = nullptr; }
    }

    ModelConfig  cfg_;
    std::string  serial_port_;
    uint32_t     baudrate_;
    bool         inverted_ = false;
    ScanCallback cb_;

    std::string detected_model_name_ = "AUTO";
    std::string firmware_;
    std::optional<uint8_t>  health_status_;
    std::optional<uint16_t> health_error_code_;

    Diagnostics latest_diag_;

    std::chrono::steady_clock::time_point last_grab_{};
    bool have_last_grab_ = false;

    sl::ILidarDriver* lidar_   = nullptr;
    sl::IChannel*     channel_ = nullptr;
};

} // 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;
    if (!transport_supported(kDriverInfo, *cfg))
        return new InvalidConfigDriver(kDriverInfo,
            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
    // "AUTO" and "C1" share the same preset; the real model is read from the
    // device at open().
    const uint32_t baud = cfg->baudrate ? cfg->baudrate : 460800;
    return new RplidarDriver(apply_device_config(MODEL_RPLIDAR_C1, *cfg),
                             cfg->serial_port, baud, cfg->inverted);
}