diff --git a/.gitignore b/.gitignore index 01aed92..8469864 100644 --- a/.gitignore +++ b/.gitignore @@ -4,7 +4,7 @@ __pycache__/ *.so .Python build/ -build-cpp*/ +build-*/ CMakeFiles/ dist/ *.egg-info/ diff --git a/CMakeLists.txt b/CMakeLists.txt index a771cd2..db69d38 100644 --- 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} diff --git a/README.md b/README.md index a5de6b9..509df4d 100644 --- 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/ # 本机实机抓包，存在时自动参与回归 ``` diff --git a/src/pro_codec.cpp b/src/pro_codec.cpp index 6f2d5f3..4a5b32b 100644 --- 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 MakeCrcTable() { + std::array 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 kCrcTable = MakeCrcTable(); uint16_t GetU16Le(const uint8_t* p) { return static_cast(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 table = [] { - std::array 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 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 data(aligned); - Decrypt(encrypted, data.data(), aligned); + if (size < kLowStateDecodeSize) return false; + std::array 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; diff --git a/src/pro_codec.h b/src/pro_codec.h index a8272c0..224ac7a 100644 --- a/src/pro_codec.h +++ b/src/pro_codec.h @@ -7,7 +7,6 @@ #include #include #include -#include namespace go1_pro_internal { diff --git a/tests/cpp/benchmark_codec.cpp b/tests/cpp/benchmark_codec.cpp new file mode 100644 index 0000000..b11cfe5 --- /dev/null +++ b/tests/cpp/benchmark_codec.cpp @@ -0,0 +1,84 @@ +#include "pro_codec.h" +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using Clock = std::chrono::steady_clock; + +#ifdef GO1_PRO_BENCHMARK_CAPTURE +std::vector ReadFile(const std::string& path) { + std::ifstream stream(path, std::ios::binary); + return std::vector( + (std::istreambuf_iterator(stream)), std::istreambuf_iterator()); +} +#endif + +double NanosecondsPerIteration(Clock::time_point start, Clock::time_point end, + std::size_t iterations) { + return std::chrono::duration(end - start).count() / + static_cast(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; +} diff --git a/tests/cpp/test_compat.cpp b/tests/cpp/test_compat.cpp index e9ddec3..74a199c 100644 --- 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{};