85 lines
2.6 KiB
C++
85 lines
2.6 KiB
C++
#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;
|
|
}
|