unitree_sdk2_rs 0.1.0

Unitree SDK2 的 Rust 封装：msg 驱动的 DDS 类型生成 + CDR 序列化 + 订阅/发布/RPC
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

#include "unitree_sdk2_rs/src/lib.rs.h"
#include "unitree_sdk2_rs/gen/gen_ffi.rs.h"
#include "dds_bridge.h"
#include "cdr_util.h"
#include <unitree/robot/channel/channel_factory.hpp>
#include <unordered_map>

namespace unitree {

void boot_dds(int32_t domain_id, rust::Str network_interface, rust::Str config_file) {
  std::string iface(network_interface), cfg(config_file);
  fprintf(stderr, "[dds] domain=%d interface=%s config=%s\n", domain_id, iface.c_str(), cfg.c_str());
  if (!cfg.empty()) {
    unitree::common::JsonMap jm;
    jm["DomainId"] = unitree::common::Any(domain_id);
    jm["Config"] = unitree::common::Any(cfg);
    unitree::robot::ChannelFactory::Instance()->Init(jm);
  } else {
    unitree::robot::ChannelFactory::Instance()->Init(domain_id, iface);
  }
}

// std::vector<uint8_t> → rust::Vec<uint8_t>
static rust::Vec<uint8_t> to_rust_vec(const std::vector<uint8_t>& v) {
  rust::Vec<uint8_t> rv;
  rv.reserve(v.size());
  for (uint8_t b : v) rv.push_back(b);
  return rv;
}

// rust::Vec<uint8_t> → std::vector<uint8_t>
static std::vector<uint8_t> to_std_vec(const rust::Vec<uint8_t>& v) {
  return std::vector<uint8_t>(v.begin(), v.end());
}

// ═══════════════════════════════════════════════
// 通用订阅器：DDS struct → CDR bytes → Rust
// ═══════════════════════════════════════════════

template<typename DdsT>
Subscriber<DdsT>::Subscriber(std::string topic, std::function<void(rust::Vec<uint8_t>)> on_bytes)
    : sub_(topic) {
  sub_.InitChannel([cb = std::move(on_bytes)](const void *m) {
    const auto *data = static_cast<const DdsT*>(m);
    auto bytes = dds_to_bytes(*data);
    cb(to_rust_vec(bytes));
  }, 1);
}

// ── 全局订阅表（类型擦除，按 id 管理） ──

static std::unordered_map<int32_t, std::unique_ptr<SubscriberBase>> g_subs;
static int32_t g_next_id = 1;

int32_t subscribe_any(rust::Str topic, rust::Str type_name, rust::Box<ByteHandler> handler) {
  std::string t(topic), tn(type_name);
  auto cb = [h = std::make_shared<rust::Box<ByteHandler>>(std::move(handler))](rust::Vec<uint8_t> bytes) {
    (*h)->on_bytes(std::move(bytes));
  };
  auto sub = make_subscriber(t, tn, cb);   // 由 build.rs 生成，按类型名分发
  if (!sub) return -1;
  int32_t id = g_next_id++;
  g_subs[id] = std::move(sub);
  return id;
}

void unsubscribe(int32_t id) { g_subs.erase(id); }

// ═══════════════════════════════════════════════
// 发布器：CDR bytes → DDS struct → SDK2 发布
// ═══════════════════════════════════════════════

template<typename DdsT>
Publisher<DdsT>::Publisher(std::string t) : pub_(t) { pub_.InitChannel(); }

// ── 逐话题实现（build.rs 从 msg 生成，统一放 gen/ 目录） ──
#include "../gen/gen_topics.inc"

} // namespace unitree