cyy/go1_pro_sdk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cpp对齐官方

Browse Source
This commit is contained in:
cyy_mac
2026-07-30 15:25:48 +08:00
parent dbfcb95566
commit f8b849397d
36 changed files with 2561 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
src/loop.cpp Normal file
View File

@@ -0,0 +1,50 @@
#include "unitree_legged_sdk/loop.h"
#include <chrono>
#if defined(__linux__)
#include <pthread.h>
#include <sched.h>
#endif
namespace UNITREE_LEGGED_SDK {
Loop::~Loop() { shutdown(); }
void Loop::start() {
if (_running_atomic.exchange(true)) return;
_isrunning = true;
_thread = std::thread(&Loop::entryFunc, this);
}
void Loop::shutdown() {
_running_atomic.store(false);
_isrunning = false;
if (_thread.joinable()) _thread.join();
}
void Loop::entryFunc() {
#if defined(__linux__)
if (_bindCPU >= 0) {
cpu_set_t cpu_set;
CPU_ZERO(&cpu_set);
CPU_SET(_bindCPU, &cpu_set);
_bind_cpu_flag = pthread_setaffinity_np(
pthread_self(), sizeof(cpu_set), &cpu_set) == 0;
}
#else
(void)_bindCPU;
_bind_cpu_flag = false;
#endif
using Clock = std::chrono::steady_clock;
const auto period = std::chrono::duration_cast<Clock::duration>(
std::chrono::duration<double>(_period));
auto next = Clock::now();
while (_running_atomic.load()) {
next += period;
functionCB();
std::this_thread::sleep_until(next);
}
}
} // namespace UNITREE_LEGGED_SDK

391
src/pro_codec.cpp Normal file
View File

@@ -0,0 +1,391 @@
#include "pro_codec.h"
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <limits.h>
#include <stdexcept>
#include <string>
#if defined(__APPLE__)
#include <mach-o/dyld.h>
#elif defined(__linux__)
#include <unistd.h>
#endif
#ifndef GO1_PRO_SOURCE_STATE_FILE
#define GO1_PRO_SOURCE_STATE_FILE ""
#endif
#ifndef GO1_PRO_INSTALL_STATE_FILE
#define GO1_PRO_INSTALL_STATE_FILE ""
#endif
namespace go1_pro_internal {
namespace {
constexpr std::size_t kStateSize = 4168;
constexpr std::size_t kMotorOffset = 22;
constexpr std::size_t kMotorWireSize = 27;
constexpr std::size_t kCrcOffset = 612;
constexpr uint32_t kCrcPoly = 0x04c11db7u;
uint16_t GetU16Le(const uint8_t* p) {
return static_cast<uint16_t>(p[0]) |
static_cast<uint16_t>(static_cast<uint16_t>(p[1]) << 8);
}
int16_t GetI16Le(const uint8_t* p) {
return static_cast<int16_t>(GetU16Le(p));
}
uint32_t GetU32Le(const uint8_t* p) {
return static_cast<uint32_t>(p[0]) |
(static_cast<uint32_t>(p[1]) << 8) |
(static_cast<uint32_t>(p[2]) << 16) |
(static_cast<uint32_t>(p[3]) << 24);
}
int32_t GetI32Le(const uint8_t* p) {
return static_cast<int32_t>(GetU32Le(p));
}
float GetF32Le(const uint8_t* p) {
const uint32_t raw = GetU32Le(p);
float result;
std::memcpy(&result, &raw, sizeof(result));
return result;
}
void PutU16Le(uint8_t* p, uint16_t value) {
p[0] = static_cast<uint8_t>(value);
p[1] = static_cast<uint8_t>(value >> 8);
}
void PutU32Le(uint8_t* p, uint32_t value) {
p[0] = static_cast<uint8_t>(value);
p[1] = static_cast<uint8_t>(value >> 8);
p[2] = static_cast<uint8_t>(value >> 16);
p[3] = static_cast<uint8_t>(value >> 24);
}
void PutF32Le(uint8_t* p, float value) {
uint32_t raw;
static_assert(sizeof(raw) == sizeof(value), "float must be 32-bit");
std::memcpy(&raw, &value, sizeof(raw));
PutU32Le(p, raw);
}
double RoundHalfEven(double value, double scale) {
const double scaled = value * scale;
const double floor_value = std::floor(scaled);
const double fraction = scaled - floor_value;
constexpr double kEpsilon = 1e-12;
double rounded = floor_value;
if (fraction > 0.5 + kEpsilon) {
rounded += 1.0;
} else if (fraction >= 0.5 - kEpsilon && std::fmod(floor_value, 2.0) != 0.0) {
rounded += 1.0;
}
return rounded / scale;
}
void EncodeTau(uint8_t* p, double tau) {
tau = RoundHalfEven(tau, 100.0);
int integer = static_cast<int>(tau);
const double fraction = tau - static_cast<double>(integer);
bool negative = tau < 0.0;
if (negative) integer = 255 + integer;
if (fraction == 0.0) negative = false;
int encoded_fraction = static_cast<int>(fraction * 256.0);
if (negative) encoded_fraction = 256 + encoded_fraction;
p[0] = static_cast<uint8_t>(encoded_fraction);
p[1] = static_cast<uint8_t>(integer);
}
void EncodeKp(uint8_t* p, double kp) {
const double base_double = std::floor(kp);
const int base = static_cast<int>(base_double);
const int fraction = static_cast<int>(RoundHalfEven(kp - base_double, 10.0) * 10.0);
const int value = fraction < 5 ? base * 32 + fraction * 3
: base * 32 + (fraction - 1) * 3 + 4;
PutU16Le(p, static_cast<uint16_t>(value));
}
void EncodeKd(uint8_t* p, double kd) {
static constexpr uint8_t kFraction[10] = {0x0, 0x1, 0x3, 0x4, 0x6,
0x8, 0x9, 0xb, 0xc, 0xe};
const int integer = static_cast<int>(kd);
const int fraction = static_cast<int>(RoundHalfEven(kd - integer, 10.0) * 10.0);
const uint8_t nibble = fraction >= 0 && fraction < 10 ? kFraction[fraction] : 0;
PutU16Le(p, static_cast<uint16_t>((integer << 4) | nibble));
}
void EncodeMotor(uint8_t* p, const UNITREE_LEGGED_SDK::MotorCmd& motor) {
p[0] = motor.mode;
PutF32Le(p + 1, motor.q);
PutF32Le(p + 5, motor.dq);
EncodeTau(p + 9, motor.tau);
EncodeKp(p + 11, motor.Kp);
EncodeKd(p + 13, motor.Kd);
for (std::size_t i = 0; i < motor.reserve.size(); ++i) {
PutU32Le(p + 15 + i * 4, motor.reserve[i]);
}
}
bool ReadableFile(const std::string& path) {
if (path.empty()) return false;
std::ifstream stream(path, std::ios::binary);
return stream.good();
}
std::string ExecutableDirectory() {
#if defined(__APPLE__)
uint32_t size = 0;
_NSGetExecutablePath(nullptr, &size);
std::string path(size, '\0');
if (_NSGetExecutablePath(&path[0], &size) != 0) return {};
path.resize(std::strlen(path.c_str()));
#elif defined(__linux__)
char buffer[PATH_MAX] = {};
const ssize_t size = readlink("/proc/self/exe", buffer, sizeof(buffer) - 1);
if (size <= 0) return {};
buffer[size] = '\0';
std::string path(buffer);
#else
return {};
#endif
const std::size_t slash = path.find_last_of("/");
return slash == std::string::npos ? std::string{} : path.substr(0, slash);
}
} // namespace
void InitLowCmd(UNITREE_LEGGED_SDK::LowCmd& cmd) {
std::memset(&cmd, 0, sizeof(cmd));
cmd.head = {{0xfe, 0xef}};
cmd.levelFlag = UNITREE_LEGGED_SDK::LOWLEVEL;
cmd.bandWidth = 0x3ac0;
for (auto& motor : cmd.motorCmd) {
motor.mode = 0x0a;
motor.q = static_cast<float>(UNITREE_LEGGED_SDK::PosStopF);
motor.dq = static_cast<float>(UNITREE_LEGGED_SDK::VelStopF);
}
}
void InitHighCmd(UNITREE_LEGGED_SDK::HighCmd& cmd) {
std::memset(&cmd, 0, sizeof(cmd));
cmd.head = {{0xfe, 0xef}};
cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL;
}
ProCodec::ProCodec(const std::string& state_path) {
std::ifstream stream(state_path, std::ios::binary);
if (!stream) {
throw std::runtime_error("cannot open Go1 PRO Blowfish state file: " + state_path);
}
std::array<uint8_t, kStateSize> data{};
stream.read(reinterpret_cast<char*>(data.data()), data.size());
if (stream.gcount() != static_cast<std::streamsize>(data.size())) {
throw std::runtime_error("Go1 PRO Blowfish state file must contain at least 4168 bytes: " + state_path);
}
for (std::size_t i = 0; i < p_.size(); ++i) p_[i] = GetU32Le(data.data() + i * 4);
const uint8_t* boxes = data.data() + p_.size() * 4;
for (std::size_t box = 0; box < s_.size(); ++box) {
for (std::size_t i = 0; i < s_[box].size(); ++i) {
s_[box][i] = GetU32Le(boxes + (box * 256 + i) * 4);
}
}
}
std::string ProCodec::FindStateFile() {
const char* environment = std::getenv("GO1_PRO_BLOWFISH_STATE");
const std::string executable_dir = ExecutableDirectory();
const std::string candidates[] = {
environment ? environment : "",
GO1_PRO_SOURCE_STATE_FILE,
GO1_PRO_INSTALL_STATE_FILE,
executable_dir.empty() ? "" : executable_dir + "/../share/go1_pro_sdk/blowfish_state.bin",
executable_dir.empty() ? "" : executable_dir + "/share/go1_pro_sdk/blowfish_state.bin",
"go1_pro_sdk/_data/blowfish_state.bin",
"../share/go1_pro_sdk/blowfish_state.bin",
"/usr/local/share/go1_pro_sdk/blowfish_state.bin",
};
for (const auto& candidate : candidates) {
if (ReadableFile(candidate)) return candidate;
}
throw std::runtime_error(
"Go1 PRO Blowfish state not found; set GO1_PRO_BLOWFISH_STATE to blowfish_state.bin");
}
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];
}
}
return crc;
}
uint32_t ProCodec::F(uint32_t value) const {
return ((s_[0][(value >> 24) & 0xffu] + s_[1][(value >> 16) & 0xffu]) ^
s_[2][(value >> 8) & 0xffu]) + s_[3][value & 0xffu];
}
void ProCodec::EncryptBlock(const uint8_t* input, uint8_t* output) const {
uint32_t left = GetU32Le(input);
uint32_t right = GetU32Le(input + 4);
for (int i = 0; i < 16; ++i) {
left ^= p_[i];
right ^= F(left);
const uint32_t temporary = left; left = right; right = temporary;
}
const uint32_t temporary = left; left = right; right = temporary;
right ^= p_[16];
left ^= p_[17];
PutU32Le(output, left);
PutU32Le(output + 4, right);
}
void ProCodec::DecryptBlock(const uint8_t* input, uint8_t* output) const {
uint32_t left = GetU32Le(input);
uint32_t right = GetU32Le(input + 4);
for (int i = 17; i > 1; --i) {
left ^= p_[i];
right ^= F(left);
const uint32_t temporary = left; left = right; right = temporary;
}
const uint32_t temporary = left; left = right; right = temporary;
right ^= p_[1];
left ^= p_[0];
PutU32Le(output, left);
PutU32Le(output + 4, right);
}
void ProCodec::Encrypt(const uint8_t* input, uint8_t* output, std::size_t size) const {
if (size % 8 != 0) throw std::invalid_argument("Blowfish input must be 8-byte aligned");
for (std::size_t offset = 0; offset < size; offset += 8) {
EncryptBlock(input + offset, output + offset);
}
}
void ProCodec::Decrypt(const uint8_t* input, uint8_t* output, std::size_t size) const {
if (size % 8 != 0) throw std::invalid_argument("Blowfish input must be 8-byte aligned");
for (std::size_t offset = 0; offset < size; offset += 8) {
DecryptBlock(input + offset, output + offset);
}
}
std::array<uint8_t, kLowCmdWireSize> ProCodec::EncodeLowCmd(
const UNITREE_LEGGED_SDK::LowCmd& cmd) const {
std::array<uint8_t, kLowCmdWireSize> plain{};
plain[0] = cmd.head[0];
plain[1] = cmd.head[1];
plain[2] = cmd.levelFlag;
plain[3] = cmd.frameReserve;
for (std::size_t i = 0; i < 2; ++i) {
PutU32Le(plain.data() + 4 + i * 4, cmd.SN[i]);
PutU32Le(plain.data() + 12 + i * 4, cmd.version[i]);
}
plain[20] = static_cast<uint8_t>(cmd.bandWidth >> 8);
plain[21] = static_cast<uint8_t>(cmd.bandWidth);
for (std::size_t i = 0; i < cmd.motorCmd.size(); ++i) {
EncodeMotor(plain.data() + kMotorOffset + i * kMotorWireSize, cmd.motorCmd[i]);
}
plain[562] = cmd.bms.off;
std::copy(cmd.bms.reserve.begin(), cmd.bms.reserve.end(), plain.begin() + 563);
std::copy(cmd.wirelessRemote.begin(), cmd.wirelessRemote.end(), plain.begin() + 566);
PutU32Le(plain.data() + 606, cmd.reserve);
PutU32Le(plain.data() + kCrcOffset, Crc32(plain.data(), kCrcOffset));
std::array<uint8_t, kLowCmdWireSize> encrypted{};
Encrypt(plain.data(), encrypted.data(), encrypted.size());
return encrypted;
}
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 (data[0] != 0xfe || data[1] != 0xef ||
data[2] != UNITREE_LEGGED_SDK::LOWLEVEL || data[3] != 0) {
return false;
}
std::memset(&state, 0, sizeof(state));
state.head = {{data[0], data[1]}};
state.levelFlag = data[2];
state.frameReserve = data[3];
for (std::size_t i = 0; i < 2; ++i) {
state.SN[i] = GetU32Le(data.data() + 4 + i * 4);
state.version[i] = GetU32Le(data.data() + 12 + i * 4);
}
state.bandWidth = GetU16Le(data.data() + 20);
for (std::size_t i = 0; i < 4; ++i) state.imu.quaternion[i] = GetF32Le(data.data() + 22 + i * 4);
for (std::size_t i = 0; i < 3; ++i) {
state.imu.gyroscope[i] = GetF32Le(data.data() + 38 + i * 4);
state.imu.accelerometer[i] = GetF32Le(data.data() + 50 + i * 4);
state.imu.rpy[i] = GetF32Le(data.data() + 62 + i * 4);
}
state.imu.temperature = static_cast<int8_t>(data[74]);
for (std::size_t i = 0; i < state.motorState.size(); ++i) {
const uint8_t* motor = data.data() + 75 + i * 32;
auto& output = state.motorState[i];
output.mode = motor[0];
output.q = GetF32Le(motor + 1);
output.dq = GetF32Le(motor + 5);
output.ddq = static_cast<float>(GetI16Le(motor + 9));
output.tauEst = static_cast<float>(GetI16Le(motor + 11)) * 0.00390625f;
output.q_raw = GetF32Le(motor + 13);
output.dq_raw = GetF32Le(motor + 17);
output.ddq_raw = static_cast<float>(GetI16Le(motor + 21));
output.temperature = static_cast<int8_t>(motor[23]);
output.reserve[0] = GetU32Le(motor + 24);
output.reserve[1] = GetU32Le(motor + 28);
}
const uint8_t* bms = data.data() + 715;
state.bms.version_h = bms[0];
state.bms.version_l = bms[1];
state.bms.bms_status = bms[2];
state.bms.SOC = bms[3];
state.bms.current = GetI32Le(bms + 4);
state.bms.cycle = GetU16Le(bms + 8);
state.bms.BQ_NTC = {{static_cast<int8_t>(bms[10]), static_cast<int8_t>(bms[11])}};
state.bms.MCU_NTC = {{static_cast<int8_t>(bms[12]), static_cast<int8_t>(bms[13])}};
for (std::size_t i = 0; i < 5; ++i) state.bms.cell_vol[i] = GetU16Le(bms + 14 + i * 2);
// The PRO state packs these fields differently from the public EDU struct.
// Keep the reverse-engineered mapping used by the Python implementation.
const std::size_t force_offsets[4] = {739, 751, 753, 755};
const std::size_t force_est_offsets[4] = {747, 759, 761, 763};
for (std::size_t i = 0; i < 4; ++i) {
state.footForce[i] = GetI16Le(data.data() + force_offsets[i]);
state.footForceEst[i] = GetI16Le(data.data() + force_est_offsets[i]);
}
std::copy(data.begin() + 759, data.begin() + 799, state.wirelessRemote.begin());
state.reserve = GetU32Le(data.data() + 799);
state.crc = GetU32Le(data.data() + 803);
return true;
}
} // namespace go1_pro_internal

48
src/pro_codec.h Normal file
View File

@@ -0,0 +1,48 @@
#ifndef GO1_PRO_SDK_PRO_CODEC_H_
#define GO1_PRO_SDK_PRO_CODEC_H_
#include "unitree_legged_sdk/comm.h"
#include <array>
#include <cstddef>
#include <cstdint>
#include <string>
#include <vector>
namespace go1_pro_internal {
constexpr std::size_t kLowCmdWireSize = 616;
constexpr std::size_t kLowStateDatagramSize = 858;
constexpr std::size_t kLowStateEncryptedSize = 856;
constexpr std::size_t kLowStateParsedSize = 807;
void InitLowCmd(UNITREE_LEGGED_SDK::LowCmd& cmd);
void InitHighCmd(UNITREE_LEGGED_SDK::HighCmd& cmd);
class ProCodec {
public:
explicit ProCodec(const std::string& state_path);
static std::string FindStateFile();
static uint32_t Crc32(const uint8_t* data, std::size_t size);
std::array<uint8_t, kLowCmdWireSize> EncodeLowCmd(
const UNITREE_LEGGED_SDK::LowCmd& cmd) const;
bool DecodeLowState(const uint8_t* encrypted, std::size_t size,
UNITREE_LEGGED_SDK::LowState& state) const;
void Encrypt(const uint8_t* input, uint8_t* output, std::size_t size) const;
void Decrypt(const uint8_t* input, uint8_t* output, std::size_t size) const;
private:
uint32_t F(uint32_t value) const;
void EncryptBlock(const uint8_t* input, uint8_t* output) const;
void DecryptBlock(const uint8_t* input, uint8_t* output) const;
std::array<uint32_t, 18> p_{};
std::array<std::array<uint32_t, 256>, 4> s_{};
};
} // namespace go1_pro_internal
#endif

25
src/quadruped.cpp Normal file
View File

@@ -0,0 +1,25 @@
#include "unitree_legged_sdk/quadruped.h"
#include "unitree_legged_sdk/comm.h"
#if defined(__unix__) || defined(__APPLE__)
#include <sys/mman.h>
#endif
namespace UNITREE_LEGGED_SDK {
const int HIGH_CMD_LENGTH = sizeof(HighCmd);
const int HIGH_STATE_LENGTH = sizeof(HighState);
const int LOW_CMD_LENGTH = 616;
const int LOW_STATE_LENGTH = 858;
std::string VersionSDK() { return "go1-pro-compat-3.8.6"; }
int InitEnvironment() {
#if defined(__unix__) || defined(__APPLE__)
return mlockall(MCL_CURRENT | MCL_FUTURE);
#else
return 0;
#endif
}
} // namespace UNITREE_LEGGED_SDK

77
src/safety.cpp Normal file
View File

@@ -0,0 +1,77 @@
#include "unitree_legged_sdk/safety.h"
#include <algorithm>
#include <cmath>
namespace UNITREE_LEGGED_SDK {
namespace {
constexpr double kTorqueLimit[12] = {
23.7, 23.7, 35.55, 23.7, 23.7, 35.55,
23.7, 23.7, 35.55, 23.7, 23.7, 35.55,
};
bool IsPositionStop(float value) {
return std::fabs(static_cast<double>(value)) >= PosStopF * 0.1;
}
} // namespace
Safety::Safety(LeggedType type)
: WattLimit(0), Wcount(0),
Hip_max(0.78), Hip_min(-0.78),
Thigh_max(3.50), Thigh_min(-0.60),
Calf_max(-0.95), Calf_min(-2.70) {
(void)type;
}
Safety::~Safety() = default;
void Safety::PositionLimit(LowCmd& cmd) {
for (std::size_t i = 0; i < 12; ++i) {
auto& motor = cmd.motorCmd[i];
if (IsPositionStop(motor.q)) continue;
const std::size_t joint = i % 3;
const double minimum = joint == 0 ? Hip_min : (joint == 1 ? Thigh_min : Calf_min);
const double maximum = joint == 0 ? Hip_max : (joint == 1 ? Thigh_max : Calf_max);
motor.q = static_cast<float>(std::max(minimum, std::min(maximum, static_cast<double>(motor.q))));
}
}
int Safety::PowerProtect(LowCmd& cmd, LowState& state, int factor) {
if (factor < 1 || factor > 10) return -1;
WattLimit = factor;
int limited = 0;
for (std::size_t i = 0; i < 12; ++i) {
const double maximum = kTorqueLimit[i];
if (std::fabs(static_cast<double>(cmd.motorCmd[i].tau)) > maximum * 5.0 ||
std::fabs(static_cast<double>(state.motorState[i].tauEst)) > maximum) {
return -1;
}
const double limit = maximum * static_cast<double>(factor) / 10.0;
const float bounded = static_cast<float>(
std::max(-limit, std::min(limit, static_cast<double>(cmd.motorCmd[i].tau))));
if (bounded != cmd.motorCmd[i].tau) {
cmd.motorCmd[i].tau = bounded;
++limited;
}
}
Wcount += limited;
return limited;
}
int Safety::PositionProtect(LowCmd& cmd, LowState& state, double limit) {
int protected_count = 0;
for (std::size_t i = 0; i < 12; ++i) {
auto& motor = cmd.motorCmd[i];
if (IsPositionStop(motor.q)) continue;
if (std::fabs(static_cast<double>(motor.q - state.motorState[i].q)) > limit) {
motor.Kp = 0.0f;
motor.Kd = 0.0f;
++protected_count;
}
}
return protected_count;
}
} // namespace UNITREE_LEGGED_SDK

310
src/udp.cpp Normal file
View File

@@ -0,0 +1,310 @@
#include "unitree_legged_sdk/udp.h"
#include "pro_codec.h"
#include <algorithm>
#include <arpa/inet.h>
#include <cerrno>
#include <cstring>
#include <fcntl.h>
#include <memory>
#include <mutex>
#include <netinet/in.h>
#include <stdexcept>
#include <string>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>
#include <vector>
namespace UNITREE_LEGGED_SDK {
namespace {
char* Duplicate(const char* value) {
if (!value) return nullptr;
const std::size_t size = std::strlen(value) + 1;
char* result = new char[size];
std::memcpy(result, value, size);
return result;
}
std::runtime_error SocketError(const char* operation) {
return std::runtime_error(std::string(operation) + ": " + std::strerror(errno));
}
} // namespace
struct UDP::Impl {
int socket_fd = -1;
int send_length = 0;
int recv_length = 0;
RecvEnum recv_type = RecvEnum::nonBlock;
bool pro_low_level = false;
bool target_configured = false;
std::mutex send_mutex;
std::mutex recv_mutex;
std::mutex socket_recv_mutex;
std::mutex state_mutex;
std::unique_ptr<go1_pro_internal::ProCodec> codec;
std::vector<uint8_t> send_buffer;
std::vector<uint8_t> recv_buffer;
std::vector<uint8_t> wire_recv_buffer;
std::size_t received_size = 0;
LowState low_state{};
bool have_low_state = false;
};
namespace {
template <typename ImplType>
void ConfigureBlocking(ImplType& impl) {
const int flags = fcntl(impl.socket_fd, F_GETFL, 0);
if (flags < 0) throw SocketError("fcntl(F_GETFL)");
const int next = impl.recv_type == RecvEnum::nonBlock ? flags | O_NONBLOCK : flags & ~O_NONBLOCK;
if (fcntl(impl.socket_fd, F_SETFL, next) < 0) throw SocketError("fcntl(F_SETFL)");
}
template <typename ImplType>
void BindSocket(UDP& udp, ImplType& impl, uint16_t port) {
impl.socket_fd = ::socket(AF_INET, SOCK_DGRAM, 0);
if (impl.socket_fd < 0) throw SocketError("socket");
const int reuse = 1;
setsockopt(impl.socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
sockaddr_in local{};
local.sin_family = AF_INET;
local.sin_port = htons(port);
local.sin_addr.s_addr = htonl(INADDR_ANY);
if (::bind(impl.socket_fd, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
throw SocketError("bind");
}
udp.localPort = port;
udp.localIP = Duplicate("0.0.0.0");
ConfigureBlocking(impl);
}
} // namespace
UDP::UDP(uint8_t level, uint16_t port, const char* ip, uint16_t target_port)
: udpState{}, targetIP(nullptr), targetPort(0), localIP(nullptr), localPort(0),
impl_(new Impl) {
try {
if (level != LOWLEVEL) {
throw std::invalid_argument("Go1 PRO compatibility library supports LOWLEVEL UDP only");
}
impl_->pro_low_level = true;
impl_->send_length = static_cast<int>(go1_pro_internal::kLowCmdWireSize);
impl_->recv_length = static_cast<int>(go1_pro_internal::kLowStateDatagramSize);
impl_->send_buffer.resize(impl_->send_length);
impl_->recv_buffer.resize(2048);
impl_->wire_recv_buffer.resize(2048);
impl_->codec.reset(new go1_pro_internal::ProCodec(
go1_pro_internal::ProCodec::FindStateFile()));
BindSocket(*this, *impl_, port);
SetIpPort(ip, target_port);
} catch (...) {
if (impl_->socket_fd >= 0) ::close(impl_->socket_fd);
delete[] localIP;
delete impl_;
throw;
}
}
UDP::UDP(uint16_t port, const char* ip, uint16_t target_port,
int send_length, int recv_length, bool initiative_disconnect, RecvEnum recv_type)
: udpState{}, targetIP(nullptr), targetPort(0), localIP(nullptr), localPort(0),
impl_(new Impl) {
(void)initiative_disconnect;
try {
impl_->send_length = send_length;
impl_->recv_length = recv_length;
impl_->recv_type = recv_type;
impl_->send_buffer.resize(send_length);
impl_->recv_buffer.resize(recv_length);
impl_->wire_recv_buffer.resize(recv_length);
BindSocket(*this, *impl_, port);
SetIpPort(ip, target_port);
} catch (...) {
if (impl_->socket_fd >= 0) ::close(impl_->socket_fd);
delete[] localIP;
delete impl_;
throw;
}
}
UDP::UDP(uint16_t port, int send_length, int recv_length,
bool initiative_disconnect, RecvEnum recv_type, bool set_ip_port)
: udpState{}, targetIP(nullptr), targetPort(0), localIP(nullptr), localPort(0),
impl_(new Impl) {
(void)initiative_disconnect;
(void)set_ip_port;
try {
impl_->send_length = send_length;
impl_->recv_length = recv_length;
impl_->recv_type = recv_type;
impl_->send_buffer.resize(send_length);
impl_->recv_buffer.resize(recv_length);
impl_->wire_recv_buffer.resize(recv_length);
BindSocket(*this, *impl_, port);
} catch (...) {
if (impl_->socket_fd >= 0) ::close(impl_->socket_fd);
delete[] localIP;
delete impl_;
throw;
}
}
UDP::~UDP() {
if (impl_) {
if (impl_->socket_fd >= 0) ::close(impl_->socket_fd);
delete impl_;
}
delete[] targetIP;
delete[] localIP;
}
void UDP::SetIpPort(const char* ip, uint16_t port) {
if (!ip) throw std::invalid_argument("targetIP must not be null");
sockaddr_in target{};
target.sin_family = AF_INET;
target.sin_port = htons(port);
if (inet_pton(AF_INET, ip, &target.sin_addr) != 1) {
throw std::invalid_argument(std::string("invalid IPv4 target: ") + ip);
}
if (::connect(impl_->socket_fd, reinterpret_cast<sockaddr*>(&target), sizeof(target)) < 0) {
throw SocketError("connect");
}
delete[] targetIP;
targetIP = Duplicate(ip);
targetPort = port;
impl_->target_configured = true;
}
void UDP::SetRecvTimeout(int time) {
timeval timeout{};
timeout.tv_sec = time / 1000;
timeout.tv_usec = (time % 1000) * 1000;
if (setsockopt(impl_->socket_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) {
throw SocketError("setsockopt(SO_RCVTIMEO)");
}
impl_->recv_type = RecvEnum::blockTimeout;
ConfigureBlocking(*impl_);
}
void UDP::SetDisconnectTime(float callback_dt, float disconnectTime) {
(void)callback_dt;
(void)disconnectTime;
}
void UDP::SetAccessibleTime(float callback_dt, float accessibleTime) {
(void)callback_dt;
(void)accessibleTime;
}
int UDP::Send() {
{
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.TotalCount;
}
std::lock_guard<std::mutex> lock(impl_->send_mutex);
if (!impl_->target_configured || impl_->send_buffer.empty()) {
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.SendError;
return -1;
}
const ssize_t sent = ::send(impl_->socket_fd, impl_->send_buffer.data(),
impl_->send_buffer.size(), 0);
if (sent < 0) {
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.SendError;
return -1;
}
{
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.SendCount;
}
return static_cast<int>(sent);
}
int UDP::Recv() {
{
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.TotalCount;
}
std::lock_guard<std::mutex> socket_lock(impl_->socket_recv_mutex);
const ssize_t received = ::recv(impl_->socket_fd, impl_->wire_recv_buffer.data(),
impl_->wire_recv_buffer.size(), 0);
if (received < 0) {
if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) return -1;
return -1;
}
LowState decoded{};
if (impl_->pro_low_level) {
if (!impl_->codec->DecodeLowState(impl_->wire_recv_buffer.data(),
static_cast<std::size_t>(received), decoded)) {
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.FlagError;
return -1;
}
}
{
std::lock_guard<std::mutex> lock(impl_->recv_mutex);
++udpState.TotalCount;
impl_->received_size = static_cast<std::size_t>(received);
impl_->recv_buffer.assign(impl_->wire_recv_buffer.begin(),
impl_->wire_recv_buffer.begin() + received);
if (impl_->pro_low_level) {
impl_->low_state = decoded;
impl_->have_low_state = true;
}
accessible = true;
}
{
std::lock_guard<std::mutex> state_lock(impl_->state_mutex);
++udpState.RecvCount;
}
return static_cast<int>(received);
}
void UDP::InitCmdData(LowCmd& cmd) {
go1_pro_internal::InitLowCmd(cmd);
}
void UDP::InitCmdData(HighCmd& cmd) {
go1_pro_internal::InitHighCmd(cmd);
}
int UDP::SetSend(char* data) {
if (!data) return -1;
std::lock_guard<std::mutex> lock(impl_->send_mutex);
std::memcpy(impl_->send_buffer.data(), data, impl_->send_buffer.size());
return 0;
}
int UDP::SetSend(LowCmd& cmd) {
if (!impl_->pro_low_level || !impl_->codec) return -1;
const auto encoded = impl_->codec->EncodeLowCmd(cmd);
std::lock_guard<std::mutex> lock(impl_->send_mutex);
impl_->send_buffer.assign(encoded.begin(), encoded.end());
return 0;
}
int UDP::SetSend(HighCmd& cmd) {
(void)cmd;
return -1;
}
void UDP::GetRecv(char* data) {
std::lock_guard<std::mutex> lock(impl_->recv_mutex);
if (!data || impl_->received_size == 0) return;
std::memcpy(data, impl_->recv_buffer.data(),
std::min(impl_->received_size, static_cast<std::size_t>(impl_->recv_length)));
}
void UDP::GetRecv(LowState& state) {
std::lock_guard<std::mutex> lock(impl_->recv_mutex);
if (impl_->have_low_state) state = impl_->low_state;
}
void UDP::GetRecv(HighState& state) { (void)state; }
} // namespace UNITREE_LEGGED_SDK