leida_simi/lidar_bridge.cpp

/**
 * lidar_bridge.cpp - C 桥接层，封装 pavo2_driver C++ 类为 C 接口供 JNA 调用
 *
 * 编译 (MinGW64):
 *   g++ -shared -std=c++11 -Wall -O2 -o lidar_bridge.dll lidar_bridge.cpp \
 *       PAVO2_SDK/src/pavo2_driver.cpp PAVO2_SDK/src/pavo2_net.cpp \
 *       PAVO2_SDK/src/pavo2_parser.cpp PAVO2_SDK/src/pavo2_filter.cpp \
 *       PAVO2_SDK/src/LdrAlgorithm.cpp \
 *       -I. -IPAVO2_SDK/include -lws2_32
 */

#include "lidar_bridge.h"
#include "pavo2_driver.h"
#include <cstring>
#include <cstdlib>

#ifdef _WIN32
#include <winsock2.h>
#endif

/* ============ 内部实现 ============ */

static pavo2::pavo2_driver* to_driver(void* handle) {
    return static_cast<pavo2::pavo2_driver*>(handle);
}

/* ============ 生命周期 ============ */

EXPORT_API void* lidar_create(void) {
#ifdef _WIN32
    WSADATA wsa;
    WSAStartup(MAKEWORD(2,2), &wsa);
#endif
    return new pavo2::pavo2_driver();
}

EXPORT_API void lidar_destroy(void* handle) {
    if (handle) {
        delete to_driver(handle);
#ifdef _WIN32
        WSACleanup();
#endif
    }
}

/* ============ 配置 ============ */

EXPORT_API void lidar_set_protocol(void* handle, int proto) {
    if (!handle) return;
    pavo2::PAVO2_PROTO_TYPE t;
    switch (proto) {
        case PROTO_UDP: t = pavo2::PAVO2_PROTO_TYPE::UDP; break;
        default:        t = pavo2::PAVO2_PROTO_TYPE::TCP; break;
    }
    to_driver(handle)->set_conn_protocol(t);
}

/* ============ 连接 ============ */

EXPORT_API int lidar_open(void* handle, const char* ip, int port, int timeout_ms) {
    if (!handle) return 0;
    return to_driver(handle)->pavo2_open(std::string(ip),
                                         (uint16_t)port,
                                         (uint32_t)timeout_ms) ? 1 : 0;
}

EXPORT_API int lidar_is_open(void* handle) {
    if (!handle) return 0;
    return to_driver(handle)->pavo2_is_open() ? 1 : 0;
}

EXPORT_API int lidar_close(void* handle) {
    if (!handle) return 0;
    return to_driver(handle)->pavo2_close() ? 1 : 0;
}

/* ============ 数据获取 ============ */

EXPORT_API int lidar_get_frame(void* handle, LidarFrame* frame, int timeout_ms) {
    if (!handle || !frame) return 0;

    frame->points    = nullptr;
    frame->count     = 0;
    frame->timestamp = 0;

    std::vector<pavo2::pavo_response_pcd_t> points;
    uint32_t ts = 0;

    if (!to_driver(handle)->get_scanned_data(points, ts, (uint16_t)timeout_ms))
        return 0;

    if (points.empty())
        return 0;

    int n = (int)points.size();
    frame->points = (LidarPoint*)malloc(n * sizeof(LidarPoint));
    if (!frame->points) return 0;

    for (int i = 0; i < n; i++) {
        frame->points[i].x         = points[i].x;
        frame->points[i].y         = points[i].y;
        frame->points[i].z         = points[i].z;
        frame->points[i].intensity = points[i].intensity;
    }
    frame->count     = n;
    frame->timestamp = ts;
    return 1;
}

EXPORT_API void lidar_free_frame(LidarFrame* frame) {
    if (frame && frame->points) {
        free(frame->points);
        frame->points = nullptr;
        frame->count  = 0;
    }
}

EXPORT_API int lidar_enable_data(void* handle, int en) {
    if (!handle) return 0;
    return to_driver(handle)->enable_data(en != 0) ? 1 : 0;
}

EXPORT_API int lidar_get_scans_available(void* handle) {
    if (!handle) return 0;
    return (int)to_driver(handle)->get_scans_available();
}

/* ============ 设备信息 ============ */

static int get_string(void* handle,
                      bool (pavo2::pavo2_driver::*fn)(std::string&),
                      char* buf, int buf_size)
{
    if (!handle || !buf || buf_size <= 0) return 0;
    std::string s;
    if (!(to_driver(handle)->*fn)(s)) return 0;
    std::strncpy(buf, s.c_str(), buf_size - 1);
    buf[buf_size - 1] = '\0';
    return 1;
}

EXPORT_API int lidar_get_sn(void* handle, char* buf, int buf_size) {
    return get_string(handle, &pavo2::pavo2_driver::get_device_sn, buf, buf_size);
}

EXPORT_API int lidar_get_pn(void* handle, char* buf, int buf_size) {
    return get_string(handle, &pavo2::pavo2_driver::get_device_pn, buf, buf_size);
}

EXPORT_API int lidar_get_sdk_ver(void* handle, char* buf, int buf_size) {
    return get_string(handle, &pavo2::pavo2_driver::get_sdk_ver, buf, buf_size);
}

EXPORT_API int lidar_get_firmware_ver(void* handle, char* buf, int buf_size) {
    return get_string(handle, &pavo2::pavo2_driver::get_firmware_ver, buf, buf_size);
}

EXPORT_API int lidar_get_error_code(void* handle) {
    if (!handle) return -1;
    return to_driver(handle)->get_error_code();
}