初始化项目，包含fast_lio_ws和unitree_sdk2

unitree_sdk2/example/h2/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.10)
+
+add_executable(h2_loco_client high_level/h2_loco_client_example.cpp)
+target_link_libraries(h2_loco_client unitree_sdk2)
+
+add_executable(h2_arm_sdk_dds_example high_level/h2_arm_sdk_dds_example.cpp)
+target_link_libraries(h2_arm_sdk_dds_example unitree_sdk2)
+
+add_executable(h2_ankle_swing_example low_level/h2_ankle_swing_example.cpp)
+target_link_libraries(h2_ankle_swing_example unitree_sdk2)
+
+find_package(yaml-cpp QUIET)
+if(yaml-cpp_FOUND AND ${yaml-cpp_VERSION} VERSION_GREATER_EQUAL "0.6")
+    add_executable(h2_dual_arm_example low_level/h2_dual_arm_example.cpp)
+    target_link_libraries(h2_dual_arm_example PRIVATE unitree_sdk2 yaml-cpp)
+    target_compile_definitions(h2_dual_arm_example PUBLIC
+        H2_DUAL_ARM_BEHAVIOR_DIR="${CMAKE_CURRENT_SOURCE_DIR}/low_level/behavior_lib/")
+endif()
+
+find_package(Boost COMPONENTS program_options)
+if(Boost_FOUND)
+    add_executable(h2_termination low_level/terminations.cpp)
+    target_link_libraries(h2_termination unitree_sdk2 Boost::program_options)
+
+    add_executable(h2_arm_action_example high_level/h2_arm_action_example.cpp)
+    target_link_libraries(h2_arm_action_example unitree_sdk2 Boost::program_options)
+endif()

unitree_sdk2/example/h2/high_level/h2_arm_action_example.cpp

@@ -0,0 +1,114 @@
+/**
+ * @file h2_arm_action_example.cpp
+ * @brief Execute a preset H2 arm action through the robot-side arm service.
+ */
+
+#include <cstdint>
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include <boost/program_options.hpp>
+
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/h2/arm/h2_arm_action_client.hpp>
+#include <unitree/robot/h2/arm/h2_arm_action_error.hpp>
+
+namespace po = boost::program_options;
+using unitree::robot::ChannelFactory;
+using namespace unitree::robot::h2;
+
+int main(int argc, const char** argv) {
+  po::options_description options("Unitree H2 Arm Action Example");
+  options.add_options()
+      ("help,h", "show help message")
+      ("network,n", po::value<std::string>()->default_value(""),
+       "DDS network interface")
+      ("list,l", "query the robot for all supported actions")
+      ("id,i", po::value<int32_t>(),
+       "preset action ID to execute; 0 lists actions and 99 releases the arms")
+      ("name", po::value<std::string>(), "custom action name to execute")
+      ("stop", "stop the current custom action");
+
+  po::variables_map args;
+  try {
+    po::store(po::parse_command_line(argc, argv, options), args);
+    po::notify(args);
+  } catch (const po::error& error) {
+    std::cerr << "Argument error: " << error.what() << "\n\n" << options << '\n';
+    return 2;
+  }
+
+  if (argc < 2 || args.count("help")) {
+    std::cout << options << '\n';
+    return 0;
+  }
+
+  if (!args.count("list") && !args.count("id") && !args.count("name") &&
+      !args.count("stop")) {
+    std::cerr << "No action selected. Use --id, --list, --name, or --stop.\n\n"
+              << options << '\n';
+    return 2;
+  }
+
+  ChannelFactory::Instance()->Init(0, args["network"].as<std::string>());
+  auto client = std::make_shared<H2ArmActionClient>();
+  client->Init();
+  client->SetTimeout(10.0f);
+
+  int32_t ret = 0;
+  if (args.count("list") ||
+      (args.count("id") && args["id"].as<int32_t>() == 0)) {
+    std::string action_list;
+    ret = client->GetActionList(action_list);
+    if (ret == 0) {
+      std::cout << "Available actions:\n" << action_list << std::endl;
+    }
+  } else if (args.count("id")) {
+    const int32_t action_id = args["id"].as<int32_t>();
+    std::cout << "Requesting H2 arm action " << action_id << "..." << std::endl;
+    ret = client->ExecuteAction(action_id);
+    if (ret == 0) {
+      std::cout << "Arm action " << action_id << " completed successfully."
+                << std::endl;
+    }
+  } else if (args.count("name")) {
+    const std::string action_name = args["name"].as<std::string>();
+    std::cout << "Requesting H2 custom arm action '" << action_name << "'..."
+              << std::endl;
+    ret = client->ExecuteAction(action_name);
+    if (ret == 0) {
+      std::cout << "Custom arm action request accepted." << std::endl;
+    }
+  } else if (args.count("stop")) {
+    ret = client->StopCustomAction();
+    if (ret == 0) {
+      std::cout << "Stop request accepted." << std::endl;
+    }
+  }
+
+  if (ret == 0) {
+    return 0;
+  }
+
+  std::cerr << "Arm action request failed, error code: " << ret << '\n';
+  switch (ret) {
+    case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
+      std::cerr << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC << '\n';
+      break;
+    case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
+      std::cerr << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << '\n';
+      break;
+    case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
+      std::cerr << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << '\n';
+      break;
+    case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
+      std::cerr << UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID_DESC << '\n';
+      break;
+    default:
+      std::cerr << "Check the DDS interface, robot-side arm service, API "
+                   "compatibility, and request parameters.\n";
+      break;
+  }
+  return 1;
+}

unitree_sdk2/example/h2/high_level/h2_arm_sdk_dds_example.cpp

@@ -0,0 +1,259 @@
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <csignal>
+#include <thread>
+
+#include <unitree/dds_wrapper/common/crc.h>
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/h2/loco/h2_loco_client.hpp>
+
+namespace {
+
+using LowCmd = unitree_hg::msg::dds_::LowCmd_;
+using LowState = unitree_hg::msg::dds_::LowState_;
+using unitree::robot::ChannelFactory;
+using unitree::robot::ChannelPublisher;
+using unitree::robot::ChannelPublisherPtr;
+using unitree::robot::ChannelSubscriber;
+using unitree::robot::ChannelSubscriberPtr;
+
+constexpr char kArmSdkTopic[] = "rt/arm_sdk";
+constexpr char kLowStateTopic[] = "rt/lowstate";
+constexpr int kArmSdkWeightJoint = 31;
+constexpr float kControlDt = 0.02f;
+constexpr float kPi = 3.14159265358979323846f;
+constexpr float kSmallShoulderRoll = 20.0f * kPi / 180.0f;
+constexpr float kSmallElbow = 30.0f * kPi / 180.0f;
+constexpr float kSmallHeadYaw = 20.0f * kPi / 180.0f;
+constexpr float kSmallHeadPitch = 15.0f * kPi / 180.0f;
+
+volatile std::sig_atomic_t g_stop_requested = 0;
+
+void HandleSignal(int) { g_stop_requested = 1; }
+
+enum H2JointIndex : int {
+  kLeftShoulderPitch = 15,
+  kLeftShoulderRoll = 16,
+  kLeftShoulderYaw = 17,
+  kLeftElbow = 18,
+  kLeftWristRoll = 19,
+  kLeftWristPitch = 20,
+  kLeftWristYaw = 21,
+  kRightShoulderPitch = 22,
+  kRightShoulderRoll = 23,
+  kRightShoulderYaw = 24,
+  kRightElbow = 25,
+  kRightWristRoll = 26,
+  kRightWristPitch = 27,
+  kRightWristYaw = 28,
+  kHeadPitch = 29,
+  kHeadYaw = 30,
+};
+
+constexpr std::array<int, 16> kUpperBodyJoints = {
+    kLeftShoulderPitch,  kLeftShoulderRoll,  kLeftShoulderYaw,
+    kLeftElbow,          kLeftWristRoll,     kLeftWristPitch,
+    kLeftWristYaw,       kRightShoulderPitch, kRightShoulderRoll,
+    kRightShoulderYaw,   kRightElbow,        kRightWristRoll,
+    kRightWristPitch,    kRightWristYaw,     kHeadPitch,
+    kHeadYaw,
+};
+
+class StateBuffer {
+ public:
+  void Set(const LowState& state) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state_ = state;
+    has_state_ = true;
+  }
+
+  bool Get(LowState& state) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!has_state_) {
+      return false;
+    }
+    state = state_;
+    return true;
+  }
+
+ private:
+  mutable std::mutex mutex_;
+  LowState state_{};
+  bool has_state_ = false;
+};
+
+void SetJointCommand(LowCmd& command, int joint, float q) {
+  auto& motor = command.motor_cmd().at(joint);
+  motor.tau(0.0f);
+  motor.q(q);
+  motor.dq(0.0f);
+  if (joint == kHeadPitch || joint == kHeadYaw) {
+    motor.kp(30.0f);
+    motor.kd(1.0f);
+  } else {
+    motor.kp(80.0f);
+    motor.kd(1.5f);
+  }
+}
+
+void Publish(LowCmd& command,
+             const ChannelPublisherPtr<LowCmd>& publisher) {
+  command.crc() = crc32_core(reinterpret_cast<uint32_t*>(&command),
+                             (sizeof(LowCmd) >> 2) - 1);
+  publisher->Write(command);
+}
+
+}  // namespace
+
+int main(int argc, char** argv) {
+  std::signal(SIGINT, HandleSignal);
+  std::signal(SIGTERM, HandleSignal);
+
+  std::cout << "WARNING: Ensure there are no obstacles around the robot.\n"
+            << "Press ENTER to continue..." << std::endl;
+  std::cin.get();
+
+  const std::string network_interface = argc > 1 ? argv[1] : "";
+  ChannelFactory::Instance()->Init(0, network_interface);
+
+  unitree::robot::h2::LocoClient loco_client;
+  loco_client.SetTimeout(5.0f);
+  loco_client.Init();
+
+  int fsm_id = -1;
+  int ret = loco_client.GetFsmId(fsm_id);
+  if (ret != 0) {
+    std::cerr << "Failed to get H2 FSM ID, error code: " << ret << std::endl;
+    return 1;
+  }
+  if (fsm_id != 4 && fsm_id != 703) {
+    std::cerr << "Current FSM " << fsm_id
+              << " does not accept rt/arm_sdk commands. Supported FSMs: 4, 703."
+              << std::endl;
+    return 1;
+  }
+
+  ret = loco_client.EnableArmSDK();
+  if (ret != 0) {
+    std::cerr << "Failed to enable the external Arm SDK, error code: " << ret
+              << std::endl;
+    return 1;
+  }
+
+  auto disable_arm_sdk = [&]() {
+    const int disable_ret = loco_client.DisableArmSDK();
+    if (disable_ret != 0) {
+      std::cerr << "Failed to disable the external Arm SDK, error code: "
+                << disable_ret << std::endl;
+    }
+  };
+
+  ChannelPublisherPtr<LowCmd> publisher =
+      std::make_shared<ChannelPublisher<LowCmd>>(kArmSdkTopic);
+  publisher->InitChannel();
+
+  auto state_buffer = std::make_shared<StateBuffer>();
+  ChannelSubscriberPtr<LowState> subscriber =
+      std::make_shared<ChannelSubscriber<LowState>>(kLowStateTopic);
+  subscriber->InitChannel(
+      [state_buffer](const void* message) {
+        state_buffer->Set(*static_cast<const LowState*>(message));
+      },
+      10);
+
+  LowState state;
+  for (int i = 0; i < 250 && !g_stop_requested && !state_buffer->Get(state);
+       ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+  if (g_stop_requested) {
+    disable_arm_sdk();
+    return 0;
+  }
+  if (!state_buffer->Get(state)) {
+    std::cerr << "Timed out waiting for rt/lowstate." << std::endl;
+    disable_arm_sdk();
+    return 1;
+  }
+
+  const std::array<float, 16> target_pos = {
+      0.0f,  kSmallShoulderRoll, 0.0f, kSmallElbow, 0.0f, 0.0f, 0.0f,
+      0.0f, -kSmallShoulderRoll,  0.0f, kSmallElbow, 0.0f, 0.0f, 0.0f,
+      kSmallHeadPitch, kSmallHeadYaw};
+  LowCmd command{};
+
+  const auto sleep_time = std::chrono::milliseconds(20);
+  // Match the Python example: every control cycle blends against the latest
+  // measured position instead of using a fixed starting pose.
+  const auto publish_joint_stage = [&](float duration, int stage) {
+    const int steps = static_cast<int>(duration / kControlDt);
+    for (int step = 0; step < steps; ++step) {
+      if (g_stop_requested) {
+        return false;
+      }
+      const float ratio = static_cast<float>(step + 1) / steps;
+      LowState latest_state;
+      if (!state_buffer->Get(latest_state)) {
+        return false;
+      }
+      for (size_t i = 0; i < kUpperBodyJoints.size(); ++i) {
+        const float measured_q =
+            latest_state.motor_state().at(kUpperBodyJoints[i]).q();
+        float q = (1.0f - ratio) * measured_q;
+        if (stage == 2) {
+          q += ratio * target_pos[i];
+        }
+        SetJointCommand(command, kUpperBodyJoints[i], q);
+      }
+      command.motor_cmd().at(kArmSdkWeightJoint).q(1.0f);
+      Publish(command, publisher);
+      std::this_thread::sleep_for(sleep_time);
+    }
+    return true;
+  };
+
+  std::cout << "[Stage 1]: set arms and head to zero posture." << std::endl;
+  if (!publish_joint_stage(3.0f, 1)) {
+    disable_arm_sdk();
+    return 0;
+  }
+
+  std::cout << "[Stage 2]: lift arms and move head." << std::endl;
+  if (!publish_joint_stage(6.0f, 2)) {
+    disable_arm_sdk();
+    return 0;
+  }
+
+  std::cout << "[Stage 3]: set arms and head back to zero posture." << std::endl;
+  if (!publish_joint_stage(9.0f, 3)) {
+    disable_arm_sdk();
+    return 0;
+  }
+
+  std::cout << "[Stage 4]: release arm_sdk." << std::endl;
+  const int release_steps = static_cast<int>(3.0f / kControlDt);
+  for (int step = 0; step < release_steps; ++step) {
+    if (g_stop_requested) {
+      break;
+    }
+    command.motor_cmd().at(kArmSdkWeightJoint).q(
+        1.0f - static_cast<float>(step + 1) / release_steps);
+    Publish(command, publisher);
+    std::this_thread::sleep_for(sleep_time);
+  }
+  command.motor_cmd().at(kArmSdkWeightJoint).q(0.0f);
+  Publish(command, publisher);
+  disable_arm_sdk();
+  std::cout << "Done!" << std::endl;
+  return 0;
+}

unitree_sdk2/example/h2/high_level/h2_loco_client_example.cpp

@@ -0,0 +1,161 @@
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include "unitree/robot/h2/loco/h2_loco_api.hpp"
+#include "unitree/robot/h2/loco/h2_loco_client.hpp"
+
+std::vector<float> stringToFloatVector(const std::string &str) {
+  std::vector<float> result;
+  std::stringstream ss(str);
+  float num;
+  while (ss >> num) {
+    result.push_back(num);
+    // ignore any trailing whitespace
+    ss.ignore();
+  }
+  return result;
+}
+
+int main(int argc, char const *argv[]) {
+  std::map<std::string, std::string> args = {{"network_interface", "lo"}};
+
+  std::map<std::string, std::string> values;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg.substr(0, 2) == "--") {
+      size_t pos = arg.find("=");
+      std::string key, value;
+      if (pos != std::string::npos) {
+        key = arg.substr(2, pos - 2);
+        value = arg.substr(pos + 1);
+
+        if (value.front() == '"' && value.back() == '"') {
+          value = value.substr(1, value.length() - 2);
+        }
+      } else {
+        key = arg.substr(2);
+        value = "";
+      }
+      if (args.find(key) != args.end()) {
+        args[key] = value;
+      } else {
+        args.insert({{key, value}});
+      }
+    }
+  }
+  std::string network_interface = args["network_interface"];
+  unitree::robot::ChannelFactory::Instance()->Init(0, network_interface);
+
+  unitree::robot::h2::LocoClient client;
+
+  client.Init();
+  client.SetTimeout(10.f);
+
+  for (const auto &arg_pair : args) {
+    std::cout << "Processing command: [" << arg_pair.first << "] with param: [" << arg_pair.second << "] ..."
+              << std::endl;
+    if (arg_pair.first == "network_interface") {
+      continue;
+    }
+
+    if (arg_pair.first == "get_fsm_id") {
+      int fsm_id;
+      client.GetFsmId(fsm_id);
+      std::cout << "current fsm_id: " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "get_fsm_mode") {
+      int fsm_mode;
+      client.GetFsmMode(fsm_mode);
+      std::cout << "current fsm_mode: " << fsm_mode << std::endl;
+    }
+
+    if (arg_pair.first == "set_fsm_id") {
+      int fsm_id = std::stoi(arg_pair.second);
+      client.SetFsmId(fsm_id);
+      std::cout << "set fsm_id to " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "set_velocity") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega, duration;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = 1.f;
+      } else if (param_size == 4) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = param.at(3);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
+        return 1;
+      }
+
+      client.SetVelocity(vx, vy, omega, duration);
+      std::cout << "set velocity to " << arg_pair.second << std::endl;
+    }
+
+    if (arg_pair.first == "damp") {
+      client.Damp();
+    }
+
+    if (arg_pair.first == "start") {
+      client.Start();
+    }
+
+    if (arg_pair.first == "stand_up") {
+      client.StandUp();
+    }
+
+    if (arg_pair.first == "zero_torque") {
+      client.ZeroTorque();
+    }
+
+    if (arg_pair.first == "stop_move") {
+      client.StopMove();
+    }
+
+    if (arg_pair.first == "switch_move_mode") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.SwitchMoveMode(flag);
+    }
+
+    if (arg_pair.first == "move") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
+        return 1;
+      }
+      client.Move(vx, vy, omega);
+    }
+
+    if (arg_pair.first == "set_speed_mode") {
+      int param = std::stoi(arg_pair.second);
+      client.SetSpeedMode(param);
+      std::cout << "set speed mode to " << arg_pair.second << std::endl;
+    }
+
+    std::cout << "Done!" << std::endl;
+  }
+
+  return 0;
+}

unitree_sdk2/example/h2/low_level/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

unitree_sdk2/example/h2/low_level/h2_ankle_swing_example.cpp

@@ -0,0 +1,380 @@
+#include <cmath>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+
+#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 H2_NUM_MOTOR = 31;
+struct ImuState {
+  std::array<float, 3> rpy = {};
+  std::array<float, 3> omega = {};
+};
+struct MotorCommand {
+  std::array<float, H2_NUM_MOTOR> q_target = {};
+  std::array<float, H2_NUM_MOTOR> dq_target = {};
+  std::array<float, H2_NUM_MOTOR> kp = {};
+  std::array<float, H2_NUM_MOTOR> kd = {};
+  std::array<float, H2_NUM_MOTOR> tau_ff = {};
+};
+struct MotorState {
+  std::array<float, H2_NUM_MOTOR> q = {};
+  std::array<float, H2_NUM_MOTOR> dq = {};
+};
+
+// Stiffness for all H2 Joints
+std::array<float, H2_NUM_MOTOR> Kp{
+    150, 150, 150, 250, 60, 90,
+    150, 150, 150, 250, 60, 90,
+    200, 200, 200,
+    90,  60,  20,  60,  4,  4,  4,
+    90,  60,  20,  60,  4,  4,  4,
+    30,  30,
+};
+
+// Damping for all H2 Joints
+std::array<float, H2_NUM_MOTOR> Kd{
+    2.0, 2.0, 2.0, 2.0, 0.3, 0.1,
+    2.0, 2.0, 2.0, 2.0, 0.3, 0.1,
+    2.5, 5.0, 5.0,
+    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+    1.0, 1.0,
+};
+
+enum class Mode {
+  PR = 0, // Series Control for Ptich/Roll Joints
+  AB = 1  // Parallel Control for A/B Joints
+};
+
+enum H2JointIndex {
+  LeftHipPitch = 0,
+  LeftHipRoll = 1,
+  LeftHipYaw = 2,
+  LeftKnee = 3,
+  LeftAnkleRoll = 4,
+  LeftAnkleRollRaw = 4,
+  LeftAnklePitch = 5,
+  LeftAnklePitchRaw = 5,
+  RightHipPitch = 6,
+  RightHipRoll = 7,
+  RightHipYaw = 8,
+  RightKnee = 9,
+  RightAnkleRoll = 10,
+  RightAnkleRollRaw = 10,
+  RightAnklePitch = 11,
+  RightAnklePitchRaw = 11,
+  WaistYaw = 12,
+  WaistRoll = 13,       
+  WaistA = 13,    
+  WaistPitch = 14,   
+  WaistB = 14,
+  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,
+  HEAD_PITCH = 29,
+  HEAD_YAW = 30,
+};
+
+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;
+};
+
+class H2Example {
+private:
+  double time_;
+  double control_dt_; // [2ms]
+  double duration_;   // [3 s]
+  int counter_;
+  Mode mode_pr_;
+  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_;
+
+  ChannelPublisherPtr<LowCmd_> lowcmd_publisher_;
+  ChannelSubscriberPtr<LowState_> lowstate_subscriber_;
+  ChannelSubscriberPtr<IMUState_> imutorso_subscriber_;
+  ThreadPtr command_writer_ptr_, control_thread_ptr_;
+
+  std::shared_ptr<unitree::robot::b2::MotionSwitcherClient> msc_;
+
+public:
+  H2Example(std::string networkInterface)
+      : time_(0.0), control_dt_(0.002), duration_(3.0), counter_(0),
+        mode_pr_(Mode::PR), mode_machine_(0) {
+    ChannelFactory::Instance()->Init(0, networkInterface);
+
+    // try to shutdown motion control-related service
+    msc_ = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
+    msc_->SetTimeout(5.0f);
+    msc_->Init();
+    std::string form, name;
+    while (msc_->CheckMode(form, name), !name.empty()) {
+      if (msc_->ReleaseMode())
+        std::cout << "Failed to switch to Release Mode\n";
+      sleep(5);
+    }
+
+    // create publisher
+    lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
+    lowcmd_publisher_->InitChannel();
+    // create subscriber
+    lowstate_subscriber_.reset(
+        new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
+    lowstate_subscriber_->InitChannel(
+        std::bind(&H2Example::LowStateHandler, this, std::placeholders::_1), 1);
+    imutorso_subscriber_.reset(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
+    imutorso_subscriber_->InitChannel(
+        std::bind(&H2Example::imuTorsoHandler, this, std::placeholders::_1), 1);
+    // create threads
+    command_writer_ptr_ =
+        CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
+                                &H2Example::LowCommandWriter, this);
+    control_thread_ptr_ = CreateRecurrentThreadEx(
+        "control", UT_CPU_ID_NONE, 2000, &H2Example::Control, this);
+  }
+
+  void imuTorsoHandler(const void *message) {
+    IMUState_ imu_torso = *(const IMUState_ *)message;
+    auto &rpy = imu_torso.rpy();
+    if (counter_ % 500 == 0)
+      printf("IMU.torso.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
+  }
+
+  void LowStateHandler(const void *message) {
+    LowState_ low_state = *(const LowState_ *)message;
+    if (low_state.crc() !=
+        Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
+      std::cout << "[ERROR] CRC Error" << std::endl;
+      return;
+    }
+
+    // get motor state
+    MotorState ms_tmp;
+    for (int i = 0; i < H2_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);
+
+    // get imu state
+    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);
+
+    // update gamepad
+    memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
+    gamepad_.update(rx_.RF_RX);
+
+    // update mode machine
+    if (mode_machine_ != low_state.mode_machine()) {
+      if (mode_machine_ == 0)
+        std::cout << "H2 type: " << unsigned(low_state.mode_machine())
+                  << std::endl;
+      mode_machine_ = low_state.mode_machine();
+    }
+
+    // report robot status every second
+    if (++counter_ % 500 == 0) {
+      counter_ = 0;
+      // IMU
+      auto &rpy = low_state.imu_state().rpy();
+      printf("IMU.pelvis.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
+
+      // RC
+      printf("gamepad_.A.pressed: %d\n", static_cast<int>(gamepad_.A.pressed));
+      printf("gamepad_.B.pressed: %d\n", static_cast<int>(gamepad_.B.pressed));
+      printf("gamepad_.X.pressed: %d\n", static_cast<int>(gamepad_.X.pressed));
+      printf("gamepad_.Y.pressed: %d\n", static_cast<int>(gamepad_.Y.pressed));
+
+      // Motor
+      auto &ms = low_state.motor_state();
+      printf("All %d Motors:", H2_NUM_MOTOR);
+      printf("\nmode: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,", ms[i].mode());
+      printf("\npos: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].q());
+      printf("\nvel: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].dq());
+      printf("\ntau_est: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].tau_est());
+      printf("\ntemperature: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%d,%d;", ms[i].temperature()[0], ms[i].temperature()[1]);
+      printf("\nvol: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].vol());
+      printf("\nsensor: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,%u;", ms[i].sensor()[0], ms[i].sensor()[1]);
+      printf("\nmotorstate: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,", ms[i].motorstate());
+      printf("\nreserve: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,%u,%u,%u;", ms[i].reserve()[0], ms[i].reserve()[1],
+               ms[i].reserve()[2], ms[i].reserve()[3]);
+      printf("\n");
+    }
+  }
+
+  void LowCommandWriter() {
+    LowCmd_ dds_low_command;
+    dds_low_command.mode_pr() = static_cast<uint8_t>(mode_pr_);
+    dds_low_command.mode_machine() = mode_machine_;
+
+    const std::shared_ptr<const MotorCommand> mc =
+        motor_command_buffer_.GetData();
+    if (mc) {
+      for (size_t i = 0; i < H2_NUM_MOTOR; i++) {
+        dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
+        dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
+        dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
+        dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
+        dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
+        dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
+      }
+
+      dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
+                                        (sizeof(dds_low_command) >> 2) - 1);
+      lowcmd_publisher_->Write(dds_low_command);
+    }
+  }
+
+  void Control() {
+    MotorCommand motor_command_tmp;
+    const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
+
+    for (int i = 0; i < H2_NUM_MOTOR; ++i) {
+      motor_command_tmp.tau_ff.at(i) = 0.0;
+      motor_command_tmp.q_target.at(i) = 0.0;
+      motor_command_tmp.dq_target.at(i) = 0.0;
+      motor_command_tmp.kp.at(i) = Kp[i];
+      motor_command_tmp.kd.at(i) = Kd[i];
+    }
+
+    if (ms) {
+      time_ += control_dt_;
+      if (time_ < duration_) {
+        // [Stage 1]: set robot to zero posture
+        for (int i = 0; i < H2_NUM_MOTOR; ++i) {
+          double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
+          motor_command_tmp.q_target.at(i) = (1.0 - ratio) * ms->q.at(i);
+        }
+      } else if (time_ < duration_ * 2) {
+        // [Stage 2]: swing ankle using PR mode
+        mode_pr_ = Mode::PR;
+        double max_P = M_PI * 30.0 / 180.0;
+        double max_R = M_PI * 10.0 / 180.0;
+        double t = time_ - duration_;
+        double L_P_des = max_P * std::sin(2.0 * M_PI * t);
+        double L_R_des = max_R * std::sin(2.0 * M_PI * t);
+        double R_P_des = max_P * std::sin(2.0 * M_PI * t);
+        double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
+
+        motor_command_tmp.q_target.at(LeftAnklePitch) = L_P_des;
+        motor_command_tmp.q_target.at(LeftAnkleRoll) = L_R_des;
+        motor_command_tmp.q_target.at(RightAnklePitch) = R_P_des;
+        motor_command_tmp.q_target.at(RightAnkleRoll) = R_R_des;
+      }
+
+      motor_command_buffer_.SetData(motor_command_tmp);
+    }
+  }
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: h2_ankle_swing_example network_interface" << std::endl;
+    exit(0);
+  }
+  std::string networkInterface = argv[1];
+  H2Example custom(networkInterface);
+  while (true)
+    sleep(10);
+  return 0;
+}
+

unitree_sdk2/example/h2/low_level/h2_dual_arm_example.cpp

@@ -0,0 +1,185 @@
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <csignal>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <yaml-cpp/yaml.h>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+namespace {
+
+using LowCmd = unitree_hg::msg::dds_::LowCmd_;
+using LowState = unitree_hg::msg::dds_::LowState_;
+constexpr char kCmdTopic[] = "rt/lowcmd";
+constexpr char kStateTopic[] = "rt/lowstate";
+constexpr int kNumMotors = 31;
+constexpr int kLeftShoulderPitch = 15;
+constexpr int kRightWristYaw = 28;
+constexpr int kHeadPitch = 29;
+constexpr int kHeadYaw = 30;
+constexpr double kControlDt = 0.002;
+constexpr double kZeroDuration = 3.0;
+// Replay the recorded trajectory at 60% of its original speed.
+constexpr double kTrajectorySpeedScale = 0.6;
+
+std::atomic<bool> g_stop{false};
+void HandleSignal(int) { g_stop.store(true); }
+
+const std::array<float, kNumMotors> kKp = {
+    150, 150, 150, 250, 60, 90, 150, 150, 150, 250, 60, 90,
+    200, 200, 200, 90, 60, 20, 60, 4, 4, 4, 90, 60, 20, 60,
+    4, 4, 4, 30, 30};
+const std::array<float, kNumMotors> kKd = {
+    2, 2, 2, 2, .3f, .1f, 2, 2, 2, 2, .3f, .1f, 2.5f, 5, 5,
+    2, 1, .4f, 1, .2f, .2f, .2f, 2, 1, .4f, 1, .2f, .2f, .2f, 1, 1};
+
+inline uint32_t Crc32Core(uint32_t* ptr, uint32_t len) {
+  uint32_t crc = 0xFFFFFFFF, polynomial = 0x04c11db7;
+  for (uint32_t i = 0; i < len; ++i) {
+    uint32_t data = ptr[i], bit = 1u << 31;
+    for (int j = 0; j < 32; ++j) {
+      crc = (crc & 0x80000000) ? (crc << 1) ^ polynomial : crc << 1;
+      if (data & bit) crc ^= polynomial;
+      bit >>= 1;
+    }
+  }
+  return crc;
+}
+
+class StateBuffer {
+ public:
+  void Set(const LowState& state) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state_ = state;
+    ready_ = true;
+  }
+  bool Get(LowState& state) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!ready_) return false;
+    state = state_;
+    return true;
+  }
+
+ private:
+  mutable std::mutex mutex_;
+  LowState state_{};
+  bool ready_ = false;
+};
+
+std::vector<std::array<float, 14>> LoadTrajectory(const std::string& path) {
+  YAML::Node root = YAML::LoadFile(path);
+  if (root["frame_rate"].as<int>() != 500) {
+    throw std::runtime_error("motion.seq must use 500 Hz");
+  }
+  for (const auto& component : root["components"]) {
+    if (component["content"].as<std::string>("") != "JointDisplacement") continue;
+    std::vector<std::array<float, 14>> frames;
+    for (const auto& frame : component["frames"]) {
+      if (frame.size() != 14) throw std::runtime_error("trajectory frame is not 14-DOF");
+      std::array<float, 14> values{};
+      for (size_t i = 0; i < values.size(); ++i) values[i] = frame[i].as<float>();
+      frames.push_back(values);
+    }
+    return frames;
+  }
+  throw std::runtime_error("JointDisplacement component not found");
+}
+
+}  // namespace
+
+int main(int argc, char** argv) {
+  if (argc < 2) {
+    std::cout << "Usage: h2_dual_arm_example network_interface" << std::endl;
+    return 0;
+  }
+  ::signal(SIGINT, HandleSignal);
+  ::signal(SIGTERM, HandleSignal);
+
+  try {
+    const std::string network = argv[1];
+    const std::string trajectory_path = std::string(H2_DUAL_ARM_BEHAVIOR_DIR) + "motion.seq";
+    const auto trajectory = LoadTrajectory(trajectory_path);
+    std::cout << "Loaded " << trajectory.size() << " frames from " << trajectory_path << std::endl;
+
+    unitree::robot::ChannelFactory::Instance()->Init(0, network);
+    auto motion_switcher = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
+    motion_switcher->SetTimeout(5.0f);
+    motion_switcher->Init();
+    std::string form, mode;
+    while (!g_stop.load() && motion_switcher->CheckMode(form, mode) == 0 && !mode.empty()) {
+      std::cout << "Releasing motion service: " << mode << std::endl;
+      if (motion_switcher->ReleaseMode() != 0) throw std::runtime_error("ReleaseMode failed");
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    auto publisher = std::make_shared<unitree::robot::ChannelPublisher<LowCmd>>(kCmdTopic);
+    publisher->InitChannel();
+    auto state_buffer = std::make_shared<StateBuffer>();
+    auto subscriber = std::make_shared<unitree::robot::ChannelSubscriber<LowState>>(kStateTopic);
+    subscriber->InitChannel([state_buffer](const void* message) {
+      const auto& state = *static_cast<const LowState*>(message);
+      if (state.crc() == Crc32Core((uint32_t*)&state, (sizeof(LowState) >> 2) - 1)) state_buffer->Set(state);
+    }, 1);
+
+    LowState state;
+    while (!g_stop.load() && !state_buffer->Get(state)) std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    if (g_stop.load()) return 0;
+
+    LowCmd command{};
+    double time = 0.0;
+    while (!g_stop.load()) {
+      if (!state_buffer->Get(state)) continue;
+      command.mode_pr() = 0;
+      command.mode_machine() = state.mode_machine();
+      const double ratio = std::clamp(time / kZeroDuration, 0.0, 1.0);
+      const size_t frame = time < kZeroDuration
+          ? 0
+          : std::min(static_cast<size_t>((time - kZeroDuration) *
+                                         kTrajectorySpeedScale / kControlDt),
+                     trajectory.size() - 1);
+      for (int i = 0; i < kNumMotors; ++i) {
+        auto& motor = command.motor_cmd().at(i);
+        motor.mode(1);
+        motor.tau(0.0f);
+        motor.dq(0.0f);
+        motor.kp(kKp[i]);
+        motor.kd(kKd[i]);
+        if (time < kZeroDuration) {
+          motor.q(static_cast<float>((1.0 - ratio) * state.motor_state().at(i).q()));
+        } else if (i >= kLeftShoulderPitch && i <= kRightWristYaw) {
+          motor.q(trajectory[frame][static_cast<size_t>(i - kLeftShoulderPitch)]);
+        } else {
+          motor.q(0.0f);
+        }
+      }
+      command.crc() = Crc32Core((uint32_t*)&command, (sizeof(LowCmd) >> 2) - 1);
+      publisher->Write(command);
+      time += kControlDt;
+      if (time >= kZeroDuration +
+                      trajectory.size() * kControlDt / kTrajectorySpeedScale) {
+        time = 0.0;
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    }
+    std::cout << "Stopped by Ctrl+C; low-level command loop exited." << std::endl;
+  } catch (const std::exception& error) {
+    std::cerr << "h2_dual_arm_example failed: " << error.what() << std::endl;
+    return 1;
+  }
+  return 0;
+}

unitree_sdk2/example/h2/low_level/terminations.cpp

@@ -0,0 +1,47 @@
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <thread>
+
+#include <boost/program_options.hpp>
+
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/h2/common/terminations.hpp>
+
+namespace po = boost::program_options;
+using unitree::robot::ChannelFactory;
+using unitree::robot::ChannelSubscriber;
+using unitree_hg::msg::dds_::LowState_;
+
+int main(int argc, char** argv) {
+  po::options_description options("Unitree H2 termination functions testing.");
+  options.add_options()
+      ("network,n", po::value<std::string>()->default_value(""),
+       "dds network interface");
+  po::variables_map args;
+  po::store(po::parse_command_line(argc, argv, options), args);
+  po::notify(args);
+  std::cout << options << std::endl;
+
+  ChannelFactory::Instance()->Init(0, args["network"].as<std::string>());
+
+  auto lowstate_subscriber =
+      std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
+  LowState_ lowstate;
+  lowstate_subscriber->InitChannel([&lowstate](const void* message) {
+    lowstate = *static_cast<const LowState_*>(message);
+  });
+
+  std::cout << "Checking terminations..." << std::endl;
+  while (true) {
+    if (unitree::robot::h2::bad_orientation(lowstate, 1.0f)) {
+      std::cout << "Bad orientation detected!" << std::endl;
+    }
+    if (unitree::robot::h2::lost_connection(lowstate_subscriber, 1000)) {
+      std::cout << "Lost connection!" << std::endl;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+}