feat: 初始化 Go2 导航相关代码与配置

unitree_sdk2/example/g1/g1d/g1_agv_client_example.cpp

@@ -0,0 +1,53 @@
+#include <unitree/robot/g1/agv/g1_agv_client.hpp>
+#include <iostream>
+#include <math.h>
+#include <unistd.h>
+
+extern "C" {
+    float sinf(float x);
+}
+
+int main()
+{
+    /*
+     * Initilaize ChannelFactory
+     */
+    unitree::robot::ChannelFactory::Instance()->Init(0);
+    unitree::robot::g1::AgvClient ac;
+
+    ac.SetTimeout(3.0f);
+    ac.Init();
+
+    //Test Api - Periodic motion
+    int cycle_count = 0;
+    const float cycle_period = 40.0f;  // 40步完成一个周期
+    const float vx_amplitude = 0.3f;
+    const float vyaw_amplitude = 0.3f;
+    const float height_amplitude = 1.0f;
+    
+    while (true)
+    {
+        // 使用正弦函数实现周期运动
+        float time_phase = (cycle_count % (int)cycle_period) / cycle_period * 2.0f * M_PI;
+        
+        // 速度周期运动: vx和vyaw使用正弦波
+        float vx = vx_amplitude * sinf(time_phase);
+        float vyaw = vyaw_amplitude * sinf(time_phase);
+        
+        int32_t ret = ac.Move(vx, 0.0f, vyaw);
+        std::cout << "Call Move vx:" << vx << " vyaw:" << vyaw << " ret:" << ret << std::endl;
+
+        usleep(50000);  // 50ms
+        
+        // 高度调节周期运动: 使用正弦波，周期与速度一致
+        float height = height_amplitude * sinf(time_phase);
+        
+        int32_t ret2 = ac.HeightAdjust(height);
+        std::cout << "Call HeightAdjust height:" << height << " ret:" << ret2 << std::endl;
+
+        cycle_count++;
+        usleep(50000);  // 50ms
+    }
+
+    return 0;
+}

unitree_sdk2/example/g1/g1d/g1d_arm_example.cpp

@@ -0,0 +1,292 @@
+#include <cmath>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+#include <thread>
+#include <chrono>
+
+#include "gamepad.hpp"
+
+// DDS
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+// IDL
+#include <unitree/idl/hg/IMUState_.hpp>
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+
+static const std::string HG_CMD_TOPIC = "rt/lowcmd";
+static const std::string HG_IMU_TORSO = "rt/secondary_imu";
+static const std::string HG_STATE_TOPIC = "rt/lowstate";
+
+using namespace unitree::common;
+using namespace unitree::robot;
+using namespace unitree_hg::msg::dds_;
+
+template <typename T>
+class DataBuffer {
+ public:
+  void SetData(const T &newData) {
+    std::unique_lock<std::shared_mutex> lock(mutex);
+    data = std::make_shared<T>(newData);
+  }
+
+  std::shared_ptr<const T> GetData() {
+    std::shared_lock<std::shared_mutex> lock(mutex);
+    return data ? data : nullptr;
+  }
+
+  void Clear() {
+    std::unique_lock<std::shared_mutex> lock(mutex);
+    data = nullptr;
+  }
+
+ private:
+  std::shared_ptr<T> data;
+  std::shared_mutex mutex;
+};
+
+const int G1D_NUM_MOTOR = 29;
+
+// IMU状态结构体
+struct ImuState {
+  std::array<float, 3> rpy = {};
+  std::array<float, 3> omega = {};
+};
+
+// 电机控制指令结构体
+struct MotorCommand {
+  std::array<float, G1D_NUM_MOTOR> q_target = {};
+  std::array<float, G1D_NUM_MOTOR> dq_target = {};
+  std::array<float, G1D_NUM_MOTOR> kp = {};
+  std::array<float, G1D_NUM_MOTOR> kd = {};
+  std::array<float, G1D_NUM_MOTOR> tau_ff = {};
+};
+
+// 电机状态结构体
+struct MotorState {
+  std::array<float, G1D_NUM_MOTOR> q = {};
+  std::array<float, G1D_NUM_MOTOR> dq = {};
+};
+
+// Stiffness for all G1D Joints
+std::array<float, G1D_NUM_MOTOR> Kp{
+    0,  0,  0,  0,  0,  0,      // empty
+    0,  0,  0,  0,  0,  0,      // empty
+    60, 0,  40,                   // waist
+    40, 40, 40, 40, 40, 40, 40,  // arms
+    40, 40, 40, 40, 40, 40, 40   // arms
+};
+
+// Damping for all G1D Joints
+std::array<float, G1D_NUM_MOTOR> Kd{
+    0, 0, 0, 0, 0, 0,     // empty
+    0, 0, 0, 0, 0, 0,     // empty
+    1, 0, 1,              // waist
+    1, 1, 1, 1, 1, 1, 1,  // arms
+    1, 1, 1, 1, 1, 1, 1   // arms
+};
+
+uint8_t mode_machine_;
+
+Gamepad gamepad_;
+REMOTE_DATA_RX rx_;
+
+DataBuffer<MotorState> motor_state_buffer_;
+DataBuffer<MotorCommand> motor_command_buffer_;
+DataBuffer<ImuState> imu_state_buffer_;
+
+ThreadPtr command_writer_ptr_, control_thread_ptr_;
+
+LowState_ g1d_low_state;
+IMUState_ Torso_IMU;
+
+bool A_PRESSED = false;
+bool IMU_Get = false;
+
+enum G1DJointIndex {
+  LeftHipPitch = 0,      // NOTE INVALID for g1d
+  LeftHipRoll = 1,       // NOTE INVALID for g1d
+  LeftHipYaw = 2,        // NOTE INVALID for g1d
+  LeftKnee = 3,          // NOTE INVALID for g1d
+  LeftAnklePitch = 4,    // NOTE INVALID for g1d
+  LeftAnkleB = 4,        // NOTE INVALID for g1d
+  LeftAnkleRoll = 5,     // NOTE INVALID for g1d
+  LeftAnkleA = 5,        // NOTE INVALID for g1d
+  RightHipPitch = 6,     // NOTE INVALID for g1d
+  RightHipRoll = 7,      // NOTE INVALID for g1d
+  RightHipYaw = 8,       // NOTE INVALID for g1d
+  RightKnee = 9,         // NOTE INVALID for g1d
+  RightAnklePitch = 10,  // NOTE INVALID for g1d
+  RightAnkleB = 10,      // NOTE INVALID for g1d
+  RightAnkleRoll = 11,   // NOTE INVALID for g1d
+  RightAnkleA = 11,      // NOTE INVALID for g1d
+  WaistYaw = 12,
+  WaistRoll = 13,        // NOTE INVALID for g1d
+  WaistA = 13,           // NOTE INVALID for g1d
+  WaistPitch = 14,
+  WaistB = 14,           // NOTE INVALID for g1d
+  LeftShoulderPitch = 15,
+  LeftShoulderRoll = 16,
+  LeftShoulderYaw = 17,
+  LeftElbow = 18,
+  LeftWristRoll = 19,
+  LeftWristPitch = 20,
+  LeftWristYaw = 21,
+  RightShoulderPitch = 22,
+  RightShoulderRoll = 23,
+  RightShoulderYaw = 24,
+  RightElbow = 25,
+  RightWristRoll = 26,
+  RightWristPitch = 27,
+  RightWristYaw = 28
+};
+
+// CRC校验值计算
+inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
+  uint32_t xbit = 0;
+  uint32_t data = 0;
+  uint32_t CRC32 = 0xFFFFFFFF;
+  const uint32_t dwPolynomial = 0x04c11db7;
+  for (uint32_t i = 0; i < len; i++) {
+    xbit = 1 << 31;
+    data = ptr[i];
+    for (uint32_t bits = 0; bits < 32; bits++) {
+      if (CRC32 & 0x80000000) {
+        CRC32 <<= 1;
+        CRC32 ^= dwPolynomial;
+      } else
+        CRC32 <<= 1;
+      if (data & xbit) CRC32 ^= dwPolynomial;
+
+      xbit >>= 1;
+    }
+  }
+  return CRC32;
+};
+
+// 躯干IMU消息回调函数
+void imuTorsoHandler(const void *message) {
+  IMUState_ imu_torso = *(const IMUState_ *)message;
+  Torso_IMU = imu_torso;
+}
+
+// 底层状态消息回调函数
+void LowStateHandler(const void *message) {
+  LowState_ low_state = *(const LowState_ *)message;
+  g1d_low_state = low_state;
+  if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
+    std::cout << "[ERROR] CRC Error" << std::endl;
+    return;
+  }
+
+  // 获取关节电机数据
+  MotorState ms_tmp;
+  for (int i = 0; i < G1D_NUM_MOTOR; ++i) {
+    ms_tmp.q.at(i) = low_state.motor_state()[i].q();
+    ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
+    if (low_state.motor_state()[i].motorstate() && i <= RightAnkleRoll)
+      std::cout << "[ERROR] motor " << i << " with code " << low_state.motor_state()[i].motorstate() << "\n";
+  }
+  motor_state_buffer_.SetData(ms_tmp);
+
+  // 获取IMU数据
+  ImuState imu_tmp;
+  imu_tmp.omega = low_state.imu_state().gyroscope();
+  imu_tmp.rpy = low_state.imu_state().rpy();
+  imu_state_buffer_.SetData(imu_tmp);
+  IMU_Get = true;
+
+  // 更新遥控器状态
+  memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
+  gamepad_.update(rx_.RF_RX);
+  if(gamepad_.A.on_press) A_PRESSED = true;
+
+  // 更新机型编号
+  mode_machine_ = low_state.mode_machine();
+}
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: g1d_arm_example network_interface" << std::endl;
+    exit(0);
+  }
+  std::string networkInterface = argv[1];
+  ChannelFactory::Instance()->Init(0, networkInterface);
+
+  // Publisher
+  ChannelPublisherPtr<LowCmd_> lowcmd_publisher(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
+  lowcmd_publisher->InitChannel();
+
+  // Subscriber
+  ChannelSubscriberPtr<LowState_> lowstate_subscriber(new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
+  lowstate_subscriber->InitChannel(LowStateHandler, 1);
+  ChannelSubscriberPtr<IMUState_> imu_subscriber(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
+  imu_subscriber->InitChannel(imuTorsoHandler, 1);
+
+  // 等待接收到第一帧IMU数据，打印躯干IMU状态信息
+  while(!IMU_Get){
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+  std::cout << "Torso IMU State: " << std::endl
+            << "RPY = " 
+            << Torso_IMU.rpy()[0] << ", " << Torso_IMU.rpy()[1] << ", " << Torso_IMU.rpy()[2]
+            << std::endl
+            << "Gyro = "
+            << Torso_IMU.gyroscope()[0] << ", " << Torso_IMU.gyroscope()[1] << ", " << Torso_IMU.gyroscope()[2]
+            << std::endl;
+
+  // 等待遥控器A键按下
+  std::cout << "Press button A on the remote control to continue!" << std::endl;
+  while(true){
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    if(A_PRESSED){
+      A_PRESSED = false;
+      break;
+    }
+  }
+  std::cout << "A Pressed!" << std::endl;
+
+  // 手臂摆动
+  LowCmd_ dds_low_command;
+  dds_low_command.mode_pr() = static_cast<uint8_t>(0); //构造底层控制指令
+  dds_low_command.mode_machine() = mode_machine_;
+  for (size_t i = 0; i < G1D_NUM_MOTOR; i++) {
+    dds_low_command.motor_cmd().at(i).mode() = 1;  // 1:Enable, 0:Disable
+    dds_low_command.motor_cmd().at(i).tau() = 0;
+    dds_low_command.motor_cmd().at(i).q() = motor_state_buffer_.GetData() -> q.at(i);
+    dds_low_command.motor_cmd().at(i).dq() = 0;
+    dds_low_command.motor_cmd().at(i).kp() = Kp.at(i);
+    dds_low_command.motor_cmd().at(i).kd() = Kd.at(i);
+  }
+
+  double control_dt = 0.002;
+  double max_P = M_PI * 30.0 / 180.0;
+  double t = 0.0;
+
+  std::cout << "Press button A on the remote control to exit the program!" << std::endl;
+  while(true){
+    double L_Shoulder_des = max_P * std::sin(2.0 * M_PI * t);
+    double R_Shoulder_des = -max_P * std::sin(2.0 * M_PI * t);
+
+    dds_low_command.motor_cmd().at(LeftShoulderPitch).q() = L_Shoulder_des;
+    dds_low_command.motor_cmd().at(RightShoulderPitch).q() = R_Shoulder_des;
+
+    dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1);
+    lowcmd_publisher -> Write(dds_low_command); //发布底层控制指令
+
+    t += control_dt;
+
+    // 按下遥控器A键或键盘Ctrl+C退出程序
+    if(A_PRESSED){
+      A_PRESSED = false;
+      break;
+    }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+  }
+
+  return 0;
+}

unitree_sdk2/example/g1/g1d/g1d_height_control.cpp

@@ -0,0 +1,184 @@
+#include <algorithm>
+#include <atomic>
+#include <cerrno>
+#include <chrono>
+#include <cmath>
+#include <csignal>
+#include <cstdint>
+#include <cstdlib>
+#include <iomanip>
+#include <iostream>
+#include <string>
+#include <thread>
+
+#include <unitree/idl/ros2/Point32_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/g1/agv/g1_agv_client.hpp>
+
+static const std::string G1D_HISPEED_STATE_TOPIC = "rt/hispeed_state";
+
+using namespace unitree::robot;
+using namespace geometry_msgs::msg::dds_;
+
+volatile std::sig_atomic_t running = 1;
+std::atomic<bool> height_received{false};
+std::atomic<float> current_height{0.0F};
+std::atomic<int64_t> last_height_time_ms{0};
+
+int64_t NowMs() {
+  using namespace std::chrono;
+  return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
+}
+
+//Process Ctrl+C interrupt signal.
+void SignalHandler(int) {
+  running = 0;
+}
+
+void HispeedStateHandler(const void *message) {
+  const Point32_ *hispeed_state = static_cast<const Point32_ *>(message);
+
+  current_height.store(hispeed_state->y(), std::memory_order_relaxed);
+  last_height_time_ms.store(NowMs(), std::memory_order_release);
+  height_received.store(true, std::memory_order_release);
+}
+
+bool WaitForHeightData() {
+  while (running != 0 && !height_received.load(std::memory_order_acquire)) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  return running != 0;
+}
+
+bool HeightDataTimeout() {
+  const int64_t last_time = last_height_time_ms.load(std::memory_order_acquire);
+  return last_time <= 0 || NowMs() - last_time > 500;
+}
+
+bool ParseFloat(const char *text, float *value) {
+  char *end = nullptr;
+  errno = 0;
+  const float parsed = std::strtof(text, &end);
+  if (text == end || *end != '\0' || errno == ERANGE || !std::isfinite(parsed)) {
+    return false;
+  }
+
+  *value = parsed;
+  return true;
+}
+
+void StopHeightAdjust(g1::AgvClient &agv_client) {
+  const int32_t ret = agv_client.HeightAdjust(0.0F);
+  if (ret != 0) {
+    std::cout << std::endl << "[WARN] HeightAdjust stop command failed, ret: " << ret << std::endl;
+  }
+}
+
+//Use PD controller to control the robot's height.
+void height_control(g1::AgvClient &agv_client, float z) {
+  constexpr float kP = 20.0F;
+  constexpr float kD = 0.3F;
+  constexpr float kMaxVz = 0.8F;
+  constexpr float kArriveTolerance = 0.001F;
+  constexpr int kArriveStableCycles = 20;
+  constexpr auto kControlPeriod = std::chrono::milliseconds(50);
+
+  if (!WaitForHeightData()) {
+    StopHeightAdjust(agv_client);
+    return;
+  }
+
+  float previous_error = z - current_height.load(std::memory_order_relaxed);
+  int arrive_count = 0;
+  auto previous_time = std::chrono::steady_clock::now();
+
+  while (running != 0) {
+    if (HeightDataTimeout()) {
+      StopHeightAdjust(agv_client);
+      std::cout << std::endl << "[ERROR] Height data timeout." << std::endl;
+      return;
+    }
+
+    const auto now = std::chrono::steady_clock::now();
+    float dt = std::chrono::duration<float>(now - previous_time).count();
+    if (dt <= 0.0F) {
+      dt = std::chrono::duration<float>(kControlPeriod).count();
+    }
+    previous_time = now;
+
+    const float height = current_height.load(std::memory_order_relaxed);
+    const float error = z - height;
+    const float d_error = (error - previous_error) / dt;
+    previous_error = error;
+
+    float vz = kP * error + kD * d_error;
+    vz = std::clamp(vz, -kMaxVz, kMaxVz);
+
+    if (std::fabs(error) <= kArriveTolerance) {
+      vz = 0.0F;
+      ++arrive_count;
+    } else {
+      arrive_count = 0;
+    }
+
+    const int32_t ret = agv_client.HeightAdjust(vz);
+    std::cout << "\rG1D column height: " << std::fixed << std::setprecision(4)
+              << height << " m"
+              << " | target: " << z << " m"
+              << " | error: " << error << " m"
+              << " | vz: " << vz << "    " << std::flush;
+
+    if (ret != 0) {
+      StopHeightAdjust(agv_client);
+      std::cout << std::endl << "[ERROR] HeightAdjust failed, ret: " << ret << std::endl;
+      return;
+    }
+
+    if (arrive_count >= kArriveStableCycles) {
+      StopHeightAdjust(agv_client);
+      std::cout << std::endl
+                << "Target height reached: " << std::fixed << std::setprecision(4)
+                << z << " m" << std::endl;
+      return;
+    }
+
+    std::this_thread::sleep_for(kControlPeriod);
+  }
+
+  StopHeightAdjust(agv_client);
+}
+
+int main(int argc, char const *argv[]) {
+  if (argc < 3) {
+    std::cout << "Usage: g1d_height_control network_interface target_height_m" << std::endl;
+    return 0;
+  }
+
+  std::signal(SIGINT, SignalHandler);
+
+  const std::string network_interface = argv[1];
+  float target_height = 0.0F;
+  if (!ParseFloat(argv[2], &target_height)) {
+    std::cout << "Invalid target_height_m: " << argv[2] << std::endl;
+    return 0;
+  }
+
+  ChannelFactory::Instance()->Init(0, network_interface);
+
+  g1::AgvClient agv_client;
+  agv_client.SetTimeout(3.0F);
+  agv_client.Init();
+
+  ChannelSubscriberPtr<Point32_> hispeed_state_subscriber(
+      new ChannelSubscriber<Point32_>(G1D_HISPEED_STATE_TOPIC));
+  hispeed_state_subscriber->InitChannel(HispeedStateHandler, 1);
+
+  std::cout << "Subscribe topic: " << G1D_HISPEED_STATE_TOPIC << std::endl;
+  std::cout << "Waiting for height data..." << std::endl;
+
+  height_control(agv_client, target_height);
+
+  std::cout << std::endl << "Exit g1d_height_control." << std::endl;
+  return 0;
+}

unitree_sdk2/example/g1/g1d/gamepad.hpp

@@ -0,0 +1,128 @@
+#pragma once
+
+#include <cmath>
+
+namespace unitree::common
+{
+
+    // bytecode mapping for raw joystick data
+    // 16b
+    typedef union
+    {
+        struct
+        {
+            uint8_t R1 : 1;
+            uint8_t L1 : 1;
+            uint8_t start : 1;
+            uint8_t select : 1;
+            uint8_t R2 : 1;
+            uint8_t L2 : 1;
+            uint8_t F1 : 1;
+            uint8_t F2 : 1;
+            uint8_t A : 1;
+            uint8_t B : 1;
+            uint8_t X : 1;
+            uint8_t Y : 1;
+            uint8_t up : 1;
+            uint8_t right : 1;
+            uint8_t down : 1;
+            uint8_t left : 1;
+        } components;
+        uint16_t value;
+    } xKeySwitchUnion;
+
+    // 40 Byte (now used 24B)
+    typedef struct
+    {
+        uint8_t head[2];
+        xKeySwitchUnion btn;
+        float lx;
+        float rx;
+        float ry;
+        float L2;
+        float ly;
+
+        uint8_t idle[16];
+    } xRockerBtnDataStruct;
+
+    typedef union
+    {
+        xRockerBtnDataStruct RF_RX;
+        uint8_t buff[40];
+    } REMOTE_DATA_RX;
+
+    class Button
+    {
+    public:
+        Button() {}
+
+        void update(bool state)
+        {
+            on_press = state ? state != pressed : false;
+            on_release = state ? false : state != pressed;
+            pressed = state;
+        }
+
+        bool pressed = false;
+        bool on_press = false;
+        bool on_release = false;
+    };
+
+    class Gamepad
+    {
+    public:
+        Gamepad() {}
+
+        void update(xRockerBtnDataStruct &key_data)
+        {
+            lx = lx * (1 - smooth) + (std::fabs(key_data.lx) < dead_zone ? 0.0 : key_data.lx) * smooth;
+            rx = rx * (1 - smooth) + (std::fabs(key_data.rx) < dead_zone ? 0.0 : key_data.rx) * smooth;
+            ry = ry * (1 - smooth) + (std::fabs(key_data.ry) < dead_zone ? 0.0 : key_data.ry) * smooth;
+            l2 = l2 * (1 - smooth) + (std::fabs(key_data.L2) < dead_zone ? 0.0 : key_data.L2) * smooth;
+            ly = ly * (1 - smooth) + (std::fabs(key_data.ly) < dead_zone ? 0.0 : key_data.ly) * smooth;
+
+            R1.update(key_data.btn.components.R1);
+            L1.update(key_data.btn.components.L1);
+            start.update(key_data.btn.components.start);
+            select.update(key_data.btn.components.select);
+            R2.update(key_data.btn.components.R2);
+            L2.update(key_data.btn.components.L2);
+            F1.update(key_data.btn.components.F1);
+            F2.update(key_data.btn.components.F2);
+            A.update(key_data.btn.components.A);
+            B.update(key_data.btn.components.B);
+            X.update(key_data.btn.components.X);
+            Y.update(key_data.btn.components.Y);
+            up.update(key_data.btn.components.up);
+            right.update(key_data.btn.components.right);
+            down.update(key_data.btn.components.down);
+            left.update(key_data.btn.components.left);
+        }
+
+        float lx = 0.;
+        float rx = 0.;
+        float ry = 0.;
+        float l2 = 0.;
+        float ly = 0.;
+
+        float smooth = 0.03;
+        float dead_zone = 0.01;
+
+        Button R1;
+        Button L1;
+        Button start;
+        Button select;
+        Button R2;
+        Button L2;
+        Button F1;
+        Button F2;
+        Button A;
+        Button B;
+        Button X;
+        Button Y;
+        Button up;
+        Button right;
+        Button down;
+        Button left;
+    };
+} // namespace unitree::common