add cpp benchmark

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;
+}

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{};