unitree_sdk2_rs 0.1.0

#pragma once
#include <vector>
#include <cstdint>
#include <org/eclipse/cyclonedds/core/cdr/basic_cdr_ser.hpp>

namespace unitree {

// ── DDS struct ↔ CDR bytes ──
// 用 Cyclone DDS 生成的 write/read 模板（ADL 发现，内部 get_type_props）。
// 输出带 4 字节 CDR 封装头（00 01 00 00 = classic CDR little-endian），
// 与 Rust 侧 cdr crate 的 CdrLe 编码逐字节一致。

// 序列化 DDS struct → CDR bytes（含封装头）
template<typename T>
std::vector<uint8_t> dds_to_bytes(const T& msg) {
    std::vector<uint8_t> buf(65536);  // 足够装下所有 hg 类型
    org::eclipse::cyclonedds::core::cdr::basic_cdr_stream s;
    s.set_buffer(buf.data(), buf.size());
    if (!write(s, msg, false)) return {};
    size_t n = s.position();
    std::vector<uint8_t> out = {0x00, 0x01, 0x00, 0x00};  // CDR_LE 封装头
    out.insert(out.end(), buf.begin(), buf.begin() + n);
    return out;
}

// 反序列化 CDR bytes → DDS struct（跳过 4 字节封装头）
template<typename T>
T bytes_to_dds(const std::vector<uint8_t>& bytes) {
    T msg{};
    if (bytes.size() <= 4) return msg;
    org::eclipse::cyclonedds::core::cdr::basic_cdr_stream s;
    s.set_buffer(const_cast<uint8_t*>(bytes.data() + 4), bytes.size() - 4);
    read(s, msg, false);
    return msg;
}

} // namespace unitree