add cpp benchmark

2026-07-30 17:25:39 +08:00
parent 3008203f82
commit bd07331734
7 changed files with 226 additions and 24 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -4,7 +4,7 @@ __pycache__/
 *.so
 .Python
 build/
-build-cpp*/
+build-*/
 CMakeFiles/
 dist/
 *.egg-info/
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(Boost REQUIRED)

 option(GO1_PRO_BUILD_EXAMPLES "Build portable official-API C++ examples" ON)
 option(GO1_PRO_BUILD_TESTS "Build C++ compatibility tests" ON)
+option(GO1_PRO_BUILD_BENCHMARKS "Build C++ codec benchmarks" OFF)

 add_library(unitree_legged_sdk
  src/loop.cpp
@@ -89,6 +90,19 @@ if(GO1_PRO_BUILD_TESTS)
  set_tests_properties(cpp_package_run PROPERTIES DEPENDS cpp_package_build)
 endif()

+if(GO1_PRO_BUILD_BENCHMARKS)
+  add_executable(benchmark_cpp_codec tests/cpp/benchmark_codec.cpp)
+  target_include_directories(benchmark_cpp_codec PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src)
+  set(GO1_PRO_BENCHMARK_CAPTURE
+      "${CMAKE_CURRENT_SOURCE_DIR}/data/captures/mcu_response_new.bin" CACHE FILEPATH
+      "Optional real-robot LowState packet used by the C++ codec benchmark")
+  if(EXISTS "${GO1_PRO_BENCHMARK_CAPTURE}")
+    target_compile_definitions(benchmark_cpp_codec PRIVATE
+      GO1_PRO_BENCHMARK_CAPTURE="${GO1_PRO_BENCHMARK_CAPTURE}")
+  endif()
+  target_link_libraries(benchmark_cpp_codec PRIVATE unitree_legged_sdk)
+endif()
+
 install(TARGETS unitree_legged_sdk EXPORT unitree_legged_sdkTargets
  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
--- a/README.md
+++ b/README.md
@@ -125,6 +125,96 @@ with MCUClient() as client:
    client.safe_stop()                       # 退出前发 damping
 ```

+## 调试与验证
+
+### 离线完整回归
+
+以下测试使用 fake socket 和 `data/captures/` 中的实机历史抓包，不连接机器狗，也不会向
+MCU 发送 UDP 数据。先在仓库根目录准备 Python 包和可选原生扩展：
+
+```bash
+conda run -n free_dog_sdk python -m pip install -e .
+cd fast_lowcmd_cpp
+PYTHONPATH=.. conda run -n free_dog_sdk python setup.py build_ext --inplace
+cd ..
+conda run -n free_dog_sdk python -m pytest -q tests fast_lowcmd_cpp/test_fast_lowcmd.py
+```
+
+C++ Release 回归会检查官方结构布局、PRO 明密文字节一致性、LowState 实机抓包、安全接口、
+Loop，以及安装后被独立 CMake 项目消费：
+
+```bash
+cmake -S . -B build-cpp -DCMAKE_BUILD_TYPE=Release \
+  -DGO1_PRO_CAPTURE_DIR="$PWD/data/captures"
+cmake --build build-cpp --parallel
+ctest --test-dir build-cpp --output-on-failure
+```
+
+配置阶段若显示 `Private data/captures fixtures not found`，说明只运行公共契约测试，未运行
+实机抓包字节回归。`ctest` 不会运行 `examples/` 中会发送命令的示例程序。
+
+### 定点调试
+
+Python 可用 pytest 节点路径只运行单项并显示输出；C++ 可直接运行兼容测试，或交给
+LLDB（macOS）/GDB（Linux）：
+
+```bash
+conda run -n free_dog_sdk python -m pytest -vv -s \
+  tests/test_capture_fixtures.py
+ctest --test-dir build-cpp -R cpp_official_compat -V
+lldb -- build-cpp/test_cpp_compat
+# Linux: gdb --args build-cpp/test_cpp_compat
+```
+
+需要验证另一份 Blowfish state 或外部抓包目录时：
+
+```bash
+GO1_PRO_BLOWFISH_STATE=/path/to/blowfish_state.bin ./build-cpp/test_cpp_compat
+cmake -S . -B build-cpp -DGO1_PRO_CAPTURE_DIR=/path/to/captures
+cmake --build build-cpp --target test_cpp_compat --parallel
+```
+
+### C++ 编译与运行时检查
+
+先用严格警告检查可移植性；ASan/UBSan 用于检查越界、生命周期和未定义行为，TSan 单独
+构建，不能与 ASan 混用：
+
+```bash
+cmake -S . -B build-cpp-warn -DCMAKE_BUILD_TYPE=Release \
+  -DCMAKE_CXX_FLAGS="-Wall -Wextra -Wpedantic"
+cmake --build build-cpp-warn --parallel
+ctest --test-dir build-cpp-warn --output-on-failure
+
+cmake -S . -B build-cpp-sanitize -DCMAKE_BUILD_TYPE=Debug \
+  -DCMAKE_CXX_FLAGS="-fsanitize=address,undefined -fno-omit-frame-pointer"
+cmake --build build-cpp-sanitize --parallel
+ctest --test-dir build-cpp-sanitize --output-on-failure
+
+cmake -S . -B build-cpp-tsan -DCMAKE_BUILD_TYPE=Debug \
+  -DCMAKE_CXX_FLAGS="-fsanitize=thread -fno-omit-frame-pointer"
+cmake --build build-cpp-tsan --parallel
+ctest --test-dir build-cpp-tsan --output-on-failure
+```
+
+### C++ 编解码基准
+
+基准默认不参与构建。存在 `data/captures/mcu_response_new.bin` 时会同时测量
+`EncodeLowCmd` 和 `DecodeLowState`；缺少抓包时只测编码：
+
+```bash
+cmake -S . -B build-cpp-benchmark -DCMAKE_BUILD_TYPE=Release \
+  -DGO1_PRO_BUILD_BENCHMARKS=ON
+cmake --build build-cpp-benchmark --target benchmark_cpp_codec --parallel
+./build-cpp-benchmark/benchmark_cpp_codec
+```
+
+### 实机烟雾验证
+
+自动测试通过后再进入下一节的实机流程。`monitor_state.py` 和 `monitor_remote.py` 虽不发送
+运动目标，但会执行 `wake_mcu()` 并持续发送 damping LowCmd 以维持回包，因此不是纯被动
+抓包；运行前仍须悬空机器狗、停掉抢占源，结束后恢复 sportMode。C++ position 示例会发送
+实际关节位置命令，只应按 [`docs/SAFETY.md`](docs/SAFETY.md) 的检查清单手动运行。
+
 ## 准备工作

 ### 1. 提取 Blowfish 密钥（仅需一次）
@@ -161,10 +251,10 @@ bash tools/stop_sportmode.sh stop
 ### 3. 跑示例

 ```bash
-# 只读监听 LowState
+# 状态监听（会发送 damping LowCmd，不是纯被动监听）
 python examples/monitor_state.py --duration 30 --verbose

-# 监听遥控器
+# 遥控器监听（会发送 damping LowCmd）
 python examples/monitor_remote.py

 # 单腿 sin 摆动 (狗悬空, 振幅 0.3 rad)
@@ -214,7 +304,7 @@ CMakeLists.txt               # C++ 构建、测试和安装入口
 examples/*.py                # Python 示例
 examples/cpp/                # C++ 同源码兼容示例
 tests/*.py                   # Python、实机抓包和 facade 回归
-tests/cpp/                   # C++ 契约与安装消费测试
+tests/cpp/                   # C++ 契约、安装消费测试与可选性能基准
 data/captures/               # 本机实机抓包，存在时自动参与回归
 ```

--- a/src/pro_codec.cpp
+++ b/src/pro_codec.cpp
@@ -31,6 +31,22 @@ constexpr std::size_t kMotorOffset = 22;
 constexpr std::size_t kMotorWireSize = 27;
 constexpr std::size_t kCrcOffset = 612;
 constexpr uint32_t kCrcPoly = 0x04c11db7u;
+constexpr std::size_t kLowStateDecodeSize =
+    ((kLowStateParsedSize + 7) / 8) * 8;
+
+std::array<uint32_t, 256> MakeCrcTable() {
+  std::array<uint32_t, 256> table{};
+  for (uint32_t byte = 0; byte < 256; ++byte) {
+    uint32_t crc = byte << 24;
+    for (int bit = 0; bit < 8; ++bit) {
+      crc = (crc & 0x80000000u) ? (crc << 1) ^ kCrcPoly : crc << 1;
+    }
+    table[byte] = crc;
+  }
+  return table;
+}
+
+const std::array<uint32_t, 256> kCrcTable = MakeCrcTable();

 uint16_t GetU16Le(const uint8_t* p) {
  return static_cast<uint16_t>(p[0]) |
@@ -221,23 +237,13 @@ std::string ProCodec::FindStateFile() {
 }

 uint32_t ProCodec::Crc32(const uint8_t* data, std::size_t size) {
-  static const std::array<uint32_t, 256> table = [] {
-    std::array<uint32_t, 256> result{};
-    for (uint32_t byte = 0; byte < 256; ++byte) {
-      uint32_t crc = byte << 24;
-      for (int bit = 0; bit < 8; ++bit) {
-        crc = (crc & 0x80000000u) ? (crc << 1) ^ kCrcPoly : crc << 1;
-      }
-      result[byte] = crc;
-    }
-    return result;
-  }();
  uint32_t crc = 0xffffffffu;
  for (std::size_t offset = 0; offset + 4 <= size; offset += 4) {
    const uint32_t word = GetU32Le(data + offset);
-    for (int shift = 24; shift >= 0; shift -= 8) {
-      crc = (crc << 8) ^ table[((crc >> 24) ^ (word >> shift)) & 0xffu];
-    }
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ (word >> 24)) & 0xffu];
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ (word >> 16)) & 0xffu];
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ (word >> 8)) & 0xffu];
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ word) & 0xffu];
  }
  return crc;
 }
@@ -320,11 +326,9 @@ std::array<uint8_t, kLowCmdWireSize> ProCodec::EncodeLowCmd(

 bool ProCodec::DecodeLowState(const uint8_t* encrypted, std::size_t size,
                              UNITREE_LEGGED_SDK::LowState& state) const {
-  if (size < kLowStateParsedSize) return false;
-  const std::size_t aligned = (size / 8) * 8;
-  if (aligned < kLowStateParsedSize) return false;
-  std::vector<uint8_t> data(aligned);
-  Decrypt(encrypted, data.data(), aligned);
+  if (size < kLowStateDecodeSize) return false;
+  std::array<uint8_t, kLowStateDecodeSize> data;
+  Decrypt(encrypted, data.data(), data.size());
  if (data[0] != 0xfe || data[1] != 0xef ||
      data[2] != UNITREE_LEGGED_SDK::LOWLEVEL || data[3] != 0) {
    return false;
--- a/src/pro_codec.h
+++ b/src/pro_codec.h
@@ -7,7 +7,6 @@
 #include <cstddef>
 #include <cstdint>
 #include <string>
-#include <vector>

 namespace go1_pro_internal {

--- a/tests/cpp/benchmark_codec.cpp
+++ b/tests/cpp/benchmark_codec.cpp
@@ -0,0 +1,84 @@
+#include "pro_codec.h"
+#include "unitree_legged_sdk/unitree_legged_sdk.h"
+
+#include <chrono>
+#include <cstdint>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <iterator>
+#include <string>
+#include <vector>
+
+namespace {
+
+using Clock = std::chrono::steady_clock;
+
+#ifdef GO1_PRO_BENCHMARK_CAPTURE
+std::vector<uint8_t> ReadFile(const std::string& path) {
+  std::ifstream stream(path, std::ios::binary);
+  return std::vector<uint8_t>(
+      (std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
+}
+#endif
+
+double NanosecondsPerIteration(Clock::time_point start, Clock::time_point end,
+                               std::size_t iterations) {
+  return std::chrono::duration<double, std::nano>(end - start).count() /
+         static_cast<double>(iterations);
+}
+
+}  // namespace
+
+int main() {
+  constexpr std::size_t kIterations = 200000;
+  go1_pro_internal::ProCodec codec(go1_pro_internal::ProCodec::FindStateFile());
+
+  UNITREE_LEGGED_SDK::LowCmd cmd{};
+  go1_pro_internal::InitLowCmd(cmd);
+  volatile uint64_t checksum = 0;
+
+  for (std::size_t i = 0; i < 100; ++i) {
+    const auto packet = codec.EncodeLowCmd(cmd);
+    checksum += packet[i % packet.size()];
+  }
+  const auto encode_start = Clock::now();
+  for (std::size_t i = 0; i < kIterations; ++i) {
+    const auto packet = codec.EncodeLowCmd(cmd);
+    checksum += packet[i % packet.size()];
+  }
+  const auto encode_end = Clock::now();
+
+  std::cout << std::fixed << std::setprecision(1)
+            << "EncodeLowCmd: "
+            << NanosecondsPerIteration(encode_start, encode_end, kIterations)
+            << " ns/frame\n";
+
+#ifdef GO1_PRO_BENCHMARK_CAPTURE
+  const auto capture = ReadFile(GO1_PRO_BENCHMARK_CAPTURE);
+  if (capture.size() != go1_pro_internal::kLowStateDatagramSize) {
+    std::cerr << "unexpected benchmark capture size: " << capture.size() << '\n';
+    return 1;
+  }
+
+  UNITREE_LEGGED_SDK::LowState state{};
+  for (std::size_t i = 0; i < 100; ++i) {
+    if (!codec.DecodeLowState(capture.data(), capture.size(), state)) return 1;
+    checksum += state.motorState[i % state.motorState.size()].temperature;
+  }
+  const auto decode_start = Clock::now();
+  for (std::size_t i = 0; i < kIterations; ++i) {
+    if (!codec.DecodeLowState(capture.data(), capture.size(), state)) return 1;
+    checksum += state.motorState[i % state.motorState.size()].temperature;
+  }
+  const auto decode_end = Clock::now();
+  std::cout << "DecodeLowState: "
+            << NanosecondsPerIteration(decode_start, decode_end, kIterations)
+            << " ns/frame\n";
+#else
+  std::cout << "DecodeLowState: skipped (capture fixture unavailable)\n";
+#endif
+
+  std::cout << "checksum: " << checksum << '\n';
+  return 0;
+}
--- a/tests/cpp/test_compat.cpp
+++ b/tests/cpp/test_compat.cpp
@@ -101,6 +101,17 @@ int main() {
  CHECK(std::isfinite(state.motorState[FR_1].q));
  CHECK(state.wirelessRemote[0] == 0x55 && state.wirelessRemote[1] == 0x51);

+  LowState truncated_state{};
+  CHECK(!codec.DecodeLowState(state_packet.data(),
+                              go1_pro_internal::kLowStateParsedSize,
+                              truncated_state));
+  CHECK(codec.DecodeLowState(state_packet.data(),
+                             go1_pro_internal::kLowStateParsedSize + 1,
+                             truncated_state));
+  CHECK(truncated_state.motorState[FR_1].temperature ==
+        state.motorState[FR_1].temperature);
+  CHECK(truncated_state.crc == state.crc);
+
  const auto old_state_packet = ReadFile(GO1_PRO_TEST_CAPTURE_OLD);
  CHECK(old_state_packet.size() == go1_pro_internal::kLowStateDatagramSize);
  LowState old_state{};