初始化项目，包含fast_lio_ws和unitree_sdk2

unitree_sdk2/include/unitree/dds_wrapper/common/Publisher.h

@@ -0,0 +1,177 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <atomic>
+#include <thread>
+#include <memory>
+
+namespace unitree
+{
+namespace robot
+{
+
+/**
+ * @brief Base class for convenient publishing to topics.
+ * This class provides an easy-to-use interface for publishing messages to topics.
+ *
+ * @tparam MessageType The type of message being published.
+ */
+template <typename MessageType>
+class PublisherBase : public unitree::robot::ChannelPublisher<MessageType>
+{
+public:
+  using MsgType = MessageType;
+  using SharedPtr = std::shared_ptr<PublisherBase<MsgType>>;
+
+  PublisherBase(std::string TOPIC_NAME)
+  : unitree::robot::ChannelPublisher<MessageType>(TOPIC_NAME)
+  {
+    this->InitChannel();
+  }
+};
+
+// For details: see https://github.com/ros-controls/realtime_tools
+template <typename MessageType>
+class RealTimePublisher
+{
+public:
+  using MsgType = MessageType;
+  using PublisherSharedPtr = typename unitree::robot::ChannelPublisherPtr<MessageType>;
+
+  MessageType msg_{};
+
+  explicit RealTimePublisher(PublisherSharedPtr publisher)
+  : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED)
+  {
+    thread_ = std::thread(&RealTimePublisher::publishingLoop, this);
+  }
+
+  explicit RealTimePublisher(std::string topic)
+  : RealTimePublisher(std::make_shared<PublisherBase<MsgType>>(topic))
+  {}
+
+  ~RealTimePublisher()
+  {
+    stop();
+    while (is_running()) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+    if(thread_.joinable()) { thread_.join(); }
+  }
+
+  void stop()
+  {
+    keep_running_ = false;
+  }
+
+  /**
+   * @brief Try to get the data lock from realtime
+   * 
+   * To publish data from the realtime loop, you need to run trylock to
+   * attenot to get unique access to the msg_ variable. Teylock returns
+   * true if the lock was aquired, and false otherwise.
+   */
+  bool trylock()
+  {
+    if(mutex_.try_lock())
+    {
+      if(turn_ == REALTIME) {
+        return true;
+      } else {
+        mutex_.unlock();
+        return false;
+      }
+    } else {
+      return false;
+    }
+  }
+
+  /**
+   * @brief Unlock the msg_ variable and publish it.
+   */
+  void unlockAndPublish() 
+  {
+    turn_ = NON_REALTIME;
+    mutex_.unlock();
+  }
+
+  /**
+   * @brief Get the data lock from non-realtime.
+   * 
+   * To publish data from the realtime loop, you need to run trylock to
+   * attenot to get unique access to the msg_ variable. Teylock returns
+   * true if the lock was aquired, and false otherwise.
+   */
+  void lock()
+  {
+    // never actually lock on the lock
+    while (!mutex_.try_lock()) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+  }
+
+  /**
+   * @brief Unlocks the data without publishing anything.
+   */
+  void unlock() { mutex_.unlock(); }
+
+protected:
+  virtual void pre_communication() {}  // something before sending the message
+  virtual void post_communication() {} // something after sending the message
+
+private:
+  // non-copyable
+  RealTimePublisher(const RealTimePublisher&) = delete;
+  RealTimePublisher& operator=(const RealTimePublisher&) = delete;
+
+
+  bool is_running() const { return is_running_; }
+
+  void publishingLoop()
+  {
+    is_running_ = true;
+    turn_ = REALTIME;
+
+    while (keep_running_)
+    {
+      MsgType outgoing;
+
+      // Locks msg_ and copies it
+      lock();
+      while (turn_ != NON_REALTIME && keep_running_)
+      {
+        unlock();
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        lock();
+      }
+      pre_communication();
+      outgoing = msg_;
+      turn_ = REALTIME;
+
+      unlock();
+
+      if(keep_running_) { 
+        publisher_->Write(outgoing, 0); 
+      }
+      post_communication();
+    }
+    is_running_ = false;
+  }
+
+  PublisherSharedPtr publisher_;
+  std::atomic_bool is_running_;
+  std::atomic_bool keep_running_;
+
+  std::mutex mutex_;
+
+  std::thread thread_;
+
+  enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
+  std::atomic<int> turn_;
+};
+
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/common/Subscription.h

@@ -0,0 +1,83 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <mutex>
+#include <thread>
+
+namespace unitree
+{
+namespace robot
+{
+/**
+ * @brief Base class for convenient subscription to topics.
+ * This class provides an easy-to-use interface for subscribing to topics, 
+ * but the continuous background updates may increase CPU usage.
+ *
+ * @tparam MessageType The type of message being subscribed to.
+ */
+template <typename MessageType>
+class SubscriptionBase
+{
+public:
+  using MsgType = MessageType;
+  using SharedPtr = std::shared_ptr<SubscriptionBase<MsgType>>;
+
+  SubscriptionBase(const std::string& topic, const std::function<void(const void*)>& handler = nullptr)
+  {
+    last_update_time_ = std::chrono::steady_clock::now() - std::chrono::milliseconds(timeout_ms_);
+    sub_ = std::make_shared<unitree::robot::ChannelSubscriber<MessageType>>(topic);
+    if (handler) {
+      sub_->InitChannel(handler);
+    } else {
+      sub_->InitChannel([this](const void *msg){
+        last_update_time_ = std::chrono::steady_clock::now();
+        std::lock_guard<std::mutex> lock(mutex_);
+        pre_communication();
+        msg_ = *(const MessageType*)msg;
+        post_communication();
+      });
+    }
+  }
+
+  void set_timeout_ms(uint32_t timeout_ms) { timeout_ms_ = timeout_ms; }
+
+  bool isTimeout() {
+    auto now = std::chrono::steady_clock::now();
+    auto elasped_time = now - last_update_time_;
+    return elasped_time > std::chrono::milliseconds(timeout_ms_);
+  }
+
+  void wait_for_connection() {
+    auto t0 = std::chrono::steady_clock::now();
+    bool warn_info = false;
+    while(isTimeout()) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      if (!warn_info && std::chrono::steady_clock::now() - t0 > std::chrono::seconds(2)) {
+        warn_info = true;
+        std::cout << "Waiting for connection " << sub_->GetChannelName() << std::endl;
+      }
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100)); // wait for stable communicaiton
+    if (warn_info) {
+      std::cout << "Connected " << sub_->GetChannelName() << std::endl;
+    }
+  }
+
+  MessageType msg_;
+  std::mutex mutex_;
+
+protected:
+  virtual void pre_communication() {}  // something before receiving message
+  virtual void post_communication() {} // something after receiving message
+
+  uint32_t timeout_ms_{1000};
+  unitree::robot::ChannelSubscriberPtr<MessageType> sub_;
+  std::chrono::steady_clock::time_point last_update_time_;
+};
+
+
+}; // namespace robot
+}; // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/common/crc.h

@@ -0,0 +1,80 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <stdint.h>
+
+inline uint16_t crc16_core (const uint8_t *nData, unsigned short wLength){
+    static const uint16_t wCRCTable[] = {
+        0X0000, 0XC0C1, 0XC181, 0X0140, 0XC301, 0X03C0, 0X0280, 0XC241,
+        0XC601, 0X06C0, 0X0780, 0XC741, 0X0500, 0XC5C1, 0XC481, 0X0440,
+        0XCC01, 0X0CC0, 0X0D80, 0XCD41, 0X0F00, 0XCFC1, 0XCE81, 0X0E40,
+        0X0A00, 0XCAC1, 0XCB81, 0X0B40, 0XC901, 0X09C0, 0X0880, 0XC841,
+        0XD801, 0X18C0, 0X1980, 0XD941, 0X1B00, 0XDBC1, 0XDA81, 0X1A40,
+        0X1E00, 0XDEC1, 0XDF81, 0X1F40, 0XDD01, 0X1DC0, 0X1C80, 0XDC41,
+        0X1400, 0XD4C1, 0XD581, 0X1540, 0XD701, 0X17C0, 0X1680, 0XD641,
+        0XD201, 0X12C0, 0X1380, 0XD341, 0X1100, 0XD1C1, 0XD081, 0X1040,
+        0XF001, 0X30C0, 0X3180, 0XF141, 0X3300, 0XF3C1, 0XF281, 0X3240,
+        0X3600, 0XF6C1, 0XF781, 0X3740, 0XF501, 0X35C0, 0X3480, 0XF441,
+        0X3C00, 0XFCC1, 0XFD81, 0X3D40, 0XFF01, 0X3FC0, 0X3E80, 0XFE41,
+        0XFA01, 0X3AC0, 0X3B80, 0XFB41, 0X3900, 0XF9C1, 0XF881, 0X3840,
+        0X2800, 0XE8C1, 0XE981, 0X2940, 0XEB01, 0X2BC0, 0X2A80, 0XEA41,
+        0XEE01, 0X2EC0, 0X2F80, 0XEF41, 0X2D00, 0XEDC1, 0XEC81, 0X2C40,
+        0XE401, 0X24C0, 0X2580, 0XE541, 0X2700, 0XE7C1, 0XE681, 0X2640,
+        0X2200, 0XE2C1, 0XE381, 0X2340, 0XE101, 0X21C0, 0X2080, 0XE041,
+        0XA001, 0X60C0, 0X6180, 0XA141, 0X6300, 0XA3C1, 0XA281, 0X6240,
+        0X6600, 0XA6C1, 0XA781, 0X6740, 0XA501, 0X65C0, 0X6480, 0XA441,
+        0X6C00, 0XACC1, 0XAD81, 0X6D40, 0XAF01, 0X6FC0, 0X6E80, 0XAE41,
+        0XAA01, 0X6AC0, 0X6B80, 0XAB41, 0X6900, 0XA9C1, 0XA881, 0X6840,
+        0X7800, 0XB8C1, 0XB981, 0X7940, 0XBB01, 0X7BC0, 0X7A80, 0XBA41,
+        0XBE01, 0X7EC0, 0X7F80, 0XBF41, 0X7D00, 0XBDC1, 0XBC81, 0X7C40,
+        0XB401, 0X74C0, 0X7580, 0XB541, 0X7700, 0XB7C1, 0XB681, 0X7640,
+        0X7200, 0XB2C1, 0XB381, 0X7340, 0XB101, 0X71C0, 0X7080, 0XB041,
+        0X5000, 0X90C1, 0X9181, 0X5140, 0X9301, 0X53C0, 0X5280, 0X9241,
+        0X9601, 0X56C0, 0X5780, 0X9741, 0X5500, 0X95C1, 0X9481, 0X5440,
+        0X9C01, 0X5CC0, 0X5D80, 0X9D41, 0X5F00, 0X9FC1, 0X9E81, 0X5E40,
+        0X5A00, 0X9AC1, 0X9B81, 0X5B40, 0X9901, 0X59C0, 0X5880, 0X9841,
+        0X8801, 0X48C0, 0X4980, 0X8941, 0X4B00, 0X8BC1, 0X8A81, 0X4A40,
+        0X4E00, 0X8EC1, 0X8F81, 0X4F40, 0X8D01, 0X4DC0, 0X4C80, 0X8C41,
+        0X4400, 0X84C1, 0X8581, 0X4540, 0X8701, 0X47C0, 0X4680, 0X8641,
+        0X8201, 0X42C0, 0X4380, 0X8341, 0X4100, 0X81C1, 0X8081, 0X4040 };
+
+    uint8_t nTemp;
+    uint16_t wCRCWord = 0xFFFF;
+
+    while (wLength--){
+        nTemp = *nData++ ^ wCRCWord;
+        wCRCWord >>= 8;
+        wCRCWord  ^= wCRCTable[nTemp];
+    }
+   return wCRCWord;
+
+} // End: CRC16
+
+inline uint32_t crc32_core(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;
+}

unitree_sdk2/include/unitree/dds_wrapper/common/unitree_joystick.hpp

@@ -0,0 +1,235 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <math.h>
+#include <stdint.h>
+#include <iostream>
+#include <chrono>
+
+namespace unitree
+{
+namespace common
+{
+    
+// ***************************** Unitree Joystick Data Type ***************************** //
+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;
+} BtnUnion;
+
+typedef struct
+{
+  uint8_t head[2];
+  BtnUnion btn;
+  float lx;
+  float rx;
+  float ry;
+  float L2;
+  float ly;
+} BtnDataStruct;         
+
+typedef union
+{
+  BtnDataStruct RF_RX;
+  uint8_t buff[40];
+}REMOTE_DATA_RX;
+
+
+// ***************************** Button & Axis ***************************** //
+class KeyBase
+{
+/**
+ * Example:
+ * >>> btn = KeyBase();
+ * 
+ * >>> btn(1); // update
+ * >>> btn.pressed
+ * >>> if btn.on_pressed:
+ * >>>   print(btn.click_cnt) 
+ * >>> if btn.pressed:
+ * >>>   print(btn.pressed_time)
+ */
+public:
+  KeyBase() = default;
+
+  bool pressed = false;
+  bool on_pressed = false;
+  bool on_released = false;
+  int click_cnt = 0; // used at `on_pressed`
+  float pressed_time = 0.0;
+
+protected:
+  void update(bool is_pressed)
+  {
+    pressed = is_pressed;
+    on_pressed = (pressed && !last_pressed_);
+    on_released = (!pressed && last_pressed_);
+    last_pressed_ = pressed;
+
+    auto now = std::chrono::steady_clock::now();
+    if (on_pressed)
+    {
+      if (std::chrono::duration<float>(now - last_click_time_).count() < double_click_threshold_) {
+        click_cnt++;
+      } else {
+        click_cnt = 1;
+      }
+      last_click_time_ = now;
+    }
+
+    if(pressed) {
+      pressed_time = std::chrono::duration<float>(now - last_click_time_).count();
+    }
+    if (!(pressed || on_released)) { // save once time on released
+      pressed_time = 0.0;
+    }
+  }
+private:
+  bool last_pressed_{false};
+  std::chrono::steady_clock::time_point last_click_time_{};
+  const float double_click_threshold_{0.5}; // Unit: second
+};
+
+template <typename T> // int, bool, string
+class Button : public KeyBase
+{
+public:
+  void operator()(const T& data){
+    bool is_pressed = (data != dataNull_);
+    update(is_pressed);
+    data_ = data;
+  }
+  const T& operator()() { return data_; }
+private:
+  T data_{}, dataNull_{};
+};
+
+class Axis : public KeyBase
+{
+public:
+  void operator()(const float& data){ // update
+    auto data_deadzone = std::fabs(data) < deadzone ? 0.0 : data;
+    float new_data = data_ * (1.0 - smooth) + data_deadzone * smooth;
+    bool is_pressed = (new_data > threshold);
+    update(is_pressed);
+    data_ = new_data;
+  }
+
+  const float& operator()() { return data_; }
+
+  float smooth = 0.03;
+  float deadzone = 0.01;
+
+  float threshold{0.5}; // Change an axis value to a button
+private:
+  float data_{};
+};
+
+// ***************************** Unitree Joystick Interface ***************************** //
+
+class UnitreeJoystick
+{
+public:
+  UnitreeJoystick() = default;
+  
+  // Adopts standard joystick key names
+  Button<int> back;
+  Button<int> start;
+  Button<int> LS;
+  Button<int> RS;
+  Button<int> LB;
+  Button<int> RB;
+  Button<int> A;
+  Button<int> B;
+  Button<int> X;
+  Button<int> Y;
+  Button<int> up;
+  Button<int> down;
+  Button<int> left;
+  Button<int> right;
+  Button<int> F1;
+  Button<int> F2;
+  Axis lx;
+  Axis ly;
+  Axis rx;
+  Axis ry;
+  Axis LT;
+  Axis RT;
+
+  virtual void update(){};
+
+  void extract(const REMOTE_DATA_RX& key)
+  {
+    back(key.RF_RX.btn.components.Select);
+    start(key.RF_RX.btn.components.Start);
+    LB(key.RF_RX.btn.components.L1);
+    RB(key.RF_RX.btn.components.R1);
+    F1(key.RF_RX.btn.components.f1);
+    F2(key.RF_RX.btn.components.f2);
+    A(key.RF_RX.btn.components.A);
+    B(key.RF_RX.btn.components.B);
+    X(key.RF_RX.btn.components.X);
+    Y(key.RF_RX.btn.components.Y);
+    up(key.RF_RX.btn.components.up);
+    down(key.RF_RX.btn.components.down);
+    left(key.RF_RX.btn.components.left);
+    right(key.RF_RX.btn.components.right);
+    LT(key.RF_RX.btn.components.L2);
+    RT(key.RF_RX.btn.components.R2);
+    lx(key.RF_RX.lx);
+    ly(key.RF_RX.ly);
+    rx(key.RF_RX.rx);
+    ry(key.RF_RX.ry);
+  }
+
+  REMOTE_DATA_RX combine()
+  {
+    REMOTE_DATA_RX key;
+    key.RF_RX.btn.components.Select = back();
+    key.RF_RX.btn.components.Start = start();
+    key.RF_RX.btn.components.L1 = LB();
+    key.RF_RX.btn.components.R1 = RB();
+    key.RF_RX.btn.components.f1 = F1();
+    key.RF_RX.btn.components.f2 = F2();
+    key.RF_RX.btn.components.A = A();
+    key.RF_RX.btn.components.B = B();
+    key.RF_RX.btn.components.X = X();
+    key.RF_RX.btn.components.Y = Y();
+    key.RF_RX.btn.components.up = up();
+    key.RF_RX.btn.components.down = down();
+    key.RF_RX.btn.components.left = left();
+    key.RF_RX.btn.components.right = right();
+    key.RF_RX.btn.components.L2 = LT() > 0.5f ? 1 : 0;
+    key.RF_RX.btn.components.R2 = RT() > 0.5f ? 1 : 0;
+    key.RF_RX.lx = lx();
+    key.RF_RX.ly = ly();
+    key.RF_RX.rx = rx();
+    key.RF_RX.ry = ry();
+    return key;
+  }
+};
+
+} // namespace common
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/g1/defines.h

@@ -0,0 +1,71 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <array>
+
+namespace unitree
+{
+namespace robot
+{
+namespace g1
+{
+
+enum JointIndex{
+    // Left leg
+    LeftHipPitch = 0,
+    LeftHipRoll = 1,
+    LeftHipYaw = 2,
+    LeftKnee = 3,
+    LeftAnklePitch = 4,
+    LeftAnkleRoll = 5,
+
+    // Right leg
+    RightHipPitch = 6,
+    RightHipRoll = 7,
+    RightHipYaw = 8,
+    RightKnee = 9,
+    RightAnklePitch = 10,
+    RightAnkleRoll = 11,
+
+    WaistYaw = 12,
+    WaistRoll = 13,
+    WaistPitch = 14,
+
+    // Left arm
+    LeftShoulderPitch = 15,
+    LeftShoulderRoll = 16,
+    LeftShoulderYaw = 17,
+    LeftElbow = 18,
+    LeftWristRoll = 19,
+    LeftWristPitch = 20,
+    LeftWristYaw = 21,
+
+    // Right arm
+    RightShoulderPitch = 22,
+    RightShoulderRoll = 23,
+    RightShoulderYaw = 24,
+    RightElbow = 25,
+    RightWristRoll = 26,
+    RightWristPitch = 27,
+    RightWristYaw = 28
+};
+
+static const std::array<int, 17> ArmJoints = {
+    JointIndex::LeftShoulderPitch,  JointIndex::LeftShoulderRoll,
+    JointIndex::LeftShoulderYaw,    JointIndex::LeftElbow,
+    JointIndex::LeftWristRoll,       JointIndex::LeftWristPitch,
+    JointIndex::LeftWristYaw,
+    JointIndex::RightShoulderPitch, JointIndex::RightShoulderRoll,
+    JointIndex::RightShoulderYaw,   JointIndex::RightElbow,
+    JointIndex::RightWristRoll,      JointIndex::RightWristPitch,
+    JointIndex::RightWristYaw,
+    JointIndex::WaistYaw,
+    JointIndex::WaistRoll,
+    JointIndex::WaistPitch
+};
+
+} // namespace g1
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/g1/g1.h

@@ -0,0 +1,18 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include "unitree/dds_wrapper/robots/g1/g1_pub.h"
+#include "unitree/dds_wrapper/robots/g1/g1_sub.h"
+
+namespace unitree
+{
+namespace robot
+{
+namespace g1
+{
+
+} // namespace g1
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/g1/g1_pub.h

@@ -0,0 +1,92 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include <unitree/dds_wrapper/common/Publisher.h>
+#include "unitree/dds_wrapper/common/crc.h"
+#include "unitree/dds_wrapper/common/unitree_joystick.hpp"
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/idl/go2/MotorCmds_.hpp>
+
+#include "g1_sub.h"
+
+
+namespace unitree
+{
+namespace robot
+{
+namespace g1
+{
+namespace publisher
+{
+
+class LowState : public RealTimePublisher<unitree_hg::msg::dds_::LowState_>
+{
+public:
+    LowState(std::string topic = "rt/lowstate") : RealTimePublisher<MsgType>(topic) 
+    {
+    }
+    std::shared_ptr<unitree::common::UnitreeJoystick> joystick = nullptr;
+
+private:
+    void pre_communication() override {
+      if (joystick) {
+        auto data = joystick->combine();
+        memcpy(&msg_.wireless_remote()[0], &data, sizeof(unitree::common::REMOTE_DATA_RX));
+      }
+      msg_.crc() = crc32_core((uint32_t*)&msg_, (sizeof(MsgType)>>2)-1);
+    }
+  };
+
+class LowCmd : public RealTimePublisher<unitree_hg::msg::dds_::LowCmd_>
+{
+public:
+    LowCmd(std::string topic = "rt/lowcmd") : RealTimePublisher<MsgType>(topic) 
+    {
+    }
+
+    bool check_mode_machine(subscription::LowState::SharedPtr lowstate = nullptr) const
+    {
+        auto sub = lowstate == nullptr ? std::make_shared<subscription::LowState>() : lowstate;
+        sub->wait_for_connection();
+        auto m_sub = sub->msg_.mode_machine();
+        auto m_pub = msg_.mode_machine();
+        
+        // 0: simulation environment
+        return !(m_sub != 0 && m_sub != m_pub);
+    }
+private:
+    /**
+     * @brief Something before sending the message.
+     */
+    void pre_communication() override {
+        msg_.crc() = crc32_core((uint32_t*)&msg_, (sizeof(MsgType)>>2)-1);
+    }
+};
+
+class ArmSdk : public RealTimePublisher<unitree_hg::msg::dds_::LowCmd_>
+{
+public:
+    ArmSdk(std::string topic = "rt/arm_sdk") : RealTimePublisher<MsgType>(topic) {}
+
+    // Enable arm sdk
+    void weight(float coe)
+    {
+        msg_.motor_cmd()[29].q() = std::clamp(coe, 0.0f, 1.0f);
+    }
+
+    float weight () const
+    {
+        return msg_.motor_cmd()[29].q();
+    }
+};
+
+
+}; // namespace publisher
+}; // namespace g1
+}; // namespace robot
+}; // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/g1/g1_sub.h

@@ -0,0 +1,111 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include "unitree/dds_wrapper/common/Subscription.h"
+#include "unitree/dds_wrapper/common/unitree_joystick.hpp"
+#include "unitree/dds_wrapper/robots/g1/defines.h"
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/idl/hg/HandCmd_.hpp>
+#include <unitree/idl/hg/HandState_.hpp>
+#include <unitree/idl/hg/SportModeState_.hpp>
+#include <unitree/idl/go2/MotorStates_.hpp>
+
+namespace unitree
+{
+namespace robot
+{
+namespace g1
+{
+namespace subscription
+{
+
+class LowCmd : public SubscriptionBase<unitree_hg::msg::dds_::LowCmd_>
+{
+public:
+    using SharedPtr = std::shared_ptr<LowCmd>;
+
+    LowCmd(std::string topic = "rt/lowcmd") : SubscriptionBase<MsgType>(topic) {}
+
+};
+
+class ArmSdk : public SubscriptionBase<unitree_hg::msg::dds_::LowCmd_>
+{
+public:
+    using SharedPtr = std::shared_ptr<LowCmd>;
+
+    ArmSdk(std::string topic = "rt/arm_sdk") : SubscriptionBase<MsgType>(topic) {}
+
+};
+class LowState : public SubscriptionBase<unitree_hg::msg::dds_::LowState_>
+{
+public:
+    using SharedPtr = std::shared_ptr<LowState>;
+
+    LowState(std::string topic = "rt/lowstate") : SubscriptionBase<MsgType>(topic) {}
+
+    void update()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        // ********** Joystick ********** //
+        // Check if all joystick values are zero to determine if the joystick is inactive
+        if(std::all_of(msg_.wireless_remote().begin(), msg_.wireless_remote().end(), [](uint8_t i){return i == 0;}))
+        {
+            auto now = std::chrono::system_clock::now();
+            auto elasped_time = now - last_joystick_time_;
+            if(elasped_time > std::chrono::milliseconds(joystick_timeout_ms_))
+            {
+                isJoystickTimeout_ = true;
+            }
+        } else {
+            last_joystick_time_ = std::chrono::system_clock::now();
+            isJoystickTimeout_ = false;
+        }
+
+        // update joystick state
+        unitree::common::REMOTE_DATA_RX key;
+        memcpy(&key, &msg_.wireless_remote()[0], 40);
+        joystick.extract(key);
+    }
+    
+    bool isJoystickTimeout() const  { return isJoystickTimeout_; }
+    unitree::common::UnitreeJoystick joystick;
+    
+private:
+    uint32_t joystick_timeout_ms_ = 3000;
+    bool isJoystickTimeout_ = false;
+    std::chrono::time_point<std::chrono::system_clock> last_joystick_time_;
+};
+
+class InspireHandState : public SubscriptionBase<unitree_go::msg::dds_::MotorStates_>
+{
+public:
+    using SharedPtr = std::shared_ptr<InspireHandState>;
+
+    InspireHandState(std::string topic = "rt/inspire/state") : SubscriptionBase<MsgType>(topic) {}
+};
+
+class Dex3LeftHandState : public SubscriptionBase<unitree_hg::msg::dds_::HandState_>
+{
+public:
+    using SharedPtr = std::shared_ptr<Dex3LeftHandState>;
+
+    Dex3LeftHandState(std::string topic = "rt/dex3/left/state") : SubscriptionBase<MsgType>(topic) {}
+};
+
+class Dex3RightHandState : public SubscriptionBase<unitree_hg::msg::dds_::HandState_>
+{
+public:
+    using SharedPtr = std::shared_ptr<Dex3RightHandState>;
+
+    Dex3RightHandState(std::string topic = "rt/dex3/right/state") : SubscriptionBase<MsgType>(topic) {}
+};
+
+} // namespace subscription
+} // namespace g1
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/go2/defines.h

@@ -0,0 +1,32 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <array>
+
+namespace unitree
+{
+namespace robot
+{
+namespace go2
+{
+
+enum class JointIndex{
+    FR_Hip = 0,
+    FR_Thigh = 1,
+    FR_Calf = 2,
+    FL_Hip = 3,
+    FL_Thigh = 4,
+    FL_Calf = 5,
+    RR_Hip = 6,
+    RR_Thigh = 7,
+    RR_Calf = 8,
+    RL_Hip = 9,
+    RL_Thigh = 10,
+    RL_Calf = 11,
+};
+
+} // namespace go2
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/go2/go2.h

@@ -0,0 +1,37 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <unitree/dds_wrapper/robots/go2/defines.h>
+#include <unitree/dds_wrapper/robots/go2/go2_pub.h>
+#include <unitree/dds_wrapper/robots/go2/go2_sub.h>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+
+namespace unitree
+{
+namespace robot
+{
+namespace go2
+{
+
+// Close the default controller
+// Run this function before publishing to rt/lowcmd.
+inline void shutdown()
+{
+    auto msc = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
+    msc->SetTimeout(3.f);
+    msc->Init();
+
+    std::string form, name;
+    while (msc->CheckMode(form, name), !name.empty()) {
+      if (msc->ReleaseMode()) {
+        std::cout << "Failed to switch to Release Mode." << std::endl;
+      }
+      sleep(3);
+    }
+}
+
+} // namespace go2
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/go2/go2_pub.h

@@ -0,0 +1,129 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include "unitree/dds_wrapper/common/Publisher.h"
+#include "unitree/dds_wrapper/common/crc.h"
+#include "unitree/dds_wrapper/common/unitree_joystick.hpp"
+
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+#include <unitree/idl/go2/SportModeCmd_.hpp>
+#include <unitree/idl/go2/SportModeState_.hpp>
+#include <unitree/idl/go2/LidarState_.hpp>
+#include <unitree/idl/go2/UwbState_.hpp>
+#include <unitree/idl/go2/HeightMap_.hpp>
+#include <unitree/idl/ros2/Time_.hpp>
+#include <unitree/idl/ros2/PointCloud2_.hpp>
+#include <unitree/idl/go2/WirelessController_.hpp>
+#include <unitree/idl/go2/Res_.hpp>
+#include <unitree/idl/go2/MotorCmds_.hpp>
+#include <unitree/idl/go2/MotorStates_.hpp>
+
+
+namespace unitree
+{
+namespace robot
+{
+namespace go2
+{ 
+namespace publisher
+{
+
+class LowCmd : public RealTimePublisher<unitree_go::msg::dds_::LowCmd_>
+{
+public:
+  LowCmd(std::string topic = "rt/lowcmd")
+  : RealTimePublisher<MsgType>(topic) 
+  {
+    msg_.head() = {0xFE, 0xEF};
+    msg_.level_flag() = 0xFF;
+
+    for (auto & m : msg_.motor_cmd()) m.mode(1);
+  } 
+
+private:
+  /**
+   * @brief Something before sending the message.
+   */
+  void pre_communication() override {
+    msg_.crc() = crc32_core((uint32_t*)&msg_, (sizeof(MsgType)>>2)-1);
+  }
+};
+
+
+class LowState : public RealTimePublisher<unitree_go::msg::dds_::LowState_>
+{
+public:
+  LowState(std::string topic = "rt/lowstate")
+  : RealTimePublisher<MsgType>(topic) 
+  {}
+
+  std::shared_ptr<unitree::common::UnitreeJoystick> joystick = nullptr;
+
+private:
+  void pre_communication() override {
+    if (joystick) {
+      unitree::common::REMOTE_DATA_RX key = joystick->combine();
+      memcpy(&msg_.wireless_remote()[0], &key, sizeof(unitree::common::REMOTE_DATA_RX));
+    }
+    msg_.crc() = crc32_core((uint32_t*)&msg_, (sizeof(MsgType)>>2)-1);
+  }
+};
+
+class SportModeState : public RealTimePublisher<unitree_go::msg::dds_::SportModeState_>
+{
+public:
+  SportModeState(std::string topic = "rt/sportmodestate")
+  : RealTimePublisher<MsgType>(topic) 
+  {
+  }
+};
+
+class WirelessController : public RealTimePublisher<unitree_go::msg::dds_::WirelessController_>
+{
+public:
+  WirelessController(std::string topic = "rt/wirelesscontroller")
+  : RealTimePublisher<MsgType>(topic) 
+  {
+  }
+
+  std::shared_ptr<unitree::common::UnitreeJoystick> joystick = nullptr;
+
+private:
+  void pre_communication() override {
+    if (joystick) {
+      msg_.lx() = joystick->lx();
+      msg_.ly() = joystick->ly();
+      msg_.rx() = joystick->rx();
+      msg_.ry() = joystick->ry();
+      msg_.keys() = joystick->combine().RF_RX.btn.value;
+    }
+  }
+};
+
+class MotorCmds : public RealTimePublisher<unitree_go::msg::dds_::MotorCmds_>
+{
+public:
+  MotorCmds(std::string topic, int num = 1)
+  : RealTimePublisher<MsgType>(topic) 
+  {
+    msg_.cmds().resize(num);
+  }
+};
+
+class MotorStates : public RealTimePublisher<unitree_go::msg::dds_::MotorStates_>
+{
+public:
+  MotorStates(std::string topic, int num = 1)
+  : RealTimePublisher<MsgType>(topic) 
+  {
+    msg_.states().resize(num);
+  }
+};
+
+} // namespace publisher
+} // namespace go2
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/go2/go2_sub.h

@@ -0,0 +1,128 @@
+// Copyright (c) 2025, Unitree Robotics Co., Ltd.
+// All rights reserved.
+
+#pragma once
+
+#include <algorithm>
+#include <eigen3/Eigen/Dense>
+#include <unordered_map>
+#include "unitree/dds_wrapper/common/Subscription.h"
+#include "unitree/dds_wrapper/common/unitree_joystick.hpp"
+
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+#include <unitree/idl/go2/SportModeCmd_.hpp>
+#include <unitree/idl/go2/SportModeState_.hpp>
+#include <unitree/idl/go2/LidarState_.hpp>
+#include <unitree/idl/go2/UwbState_.hpp>
+#include <unitree/idl/go2/HeightMap_.hpp>
+#include <unitree/idl/ros2/Time_.hpp>
+#include <unitree/idl/ros2/PointCloud2_.hpp>
+#include <unitree/idl/go2/WirelessController_.hpp>
+#include <unitree/idl/go2/MotorCmds_.hpp>
+#include <unitree/idl/go2/MotorStates_.hpp>
+
+
+namespace unitree
+{
+namespace robot
+{
+namespace go2
+{ 
+
+namespace subscription
+{
+
+class LowState : public SubscriptionBase<unitree_go::msg::dds_::LowState_>
+{
+public:
+  using SharedPtr = std::shared_ptr<LowState>;
+
+  LowState(std::string topic = "rt/lowstate") : SubscriptionBase<MsgType>(topic) {}
+
+  void update()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    // ********** Joystick ********** //
+    // Check if all joystick values are zero to determine if the joystick is inactive
+    if(std::all_of(msg_.wireless_remote().begin(), msg_.wireless_remote().end(), [](uint8_t i){return i == 0;}))
+    {
+      auto now = std::chrono::system_clock::now();
+      auto elasped_time = now - last_joystick_time_;
+      if(elasped_time > std::chrono::milliseconds(joystick_timeout_ms_))
+      {
+        isJoystickTimeout_ = true;
+      }
+    } else {
+      last_joystick_time_ = std::chrono::system_clock::now();
+      isJoystickTimeout_ = false;
+    }
+
+    // update joystick state
+    unitree::common::REMOTE_DATA_RX key;
+    memcpy(&key, &msg_.wireless_remote()[0], 40);
+    joystick.extract(key);
+  }
+
+  bool isJoystickTimeout() const  { return isJoystickTimeout_; }
+
+  unitree::common::UnitreeJoystick joystick;
+
+private:
+  uint32_t joystick_timeout_ms_ = 3000;
+  bool isJoystickTimeout_ = false;
+  std::chrono::time_point<std::chrono::system_clock> last_joystick_time_;
+};
+
+class LowCmd : public SubscriptionBase<unitree_go::msg::dds_::LowCmd_>
+{
+public:
+  using SharedPtr = std::shared_ptr<LowCmd>;
+
+  LowCmd(std::string topic = "rt/lowcmd") : SubscriptionBase<MsgType>(topic) {}
+};
+
+class SportModeState : public SubscriptionBase<unitree_go::msg::dds_::SportModeState_>
+{
+public:
+  using SharedPtr = std::shared_ptr<SportModeState>;
+
+  SportModeState(std::string topic = "rt/sportmodestate") : SubscriptionBase<MsgType>(topic) {}
+
+  const uint32_t gaitType() const { return msg_.gait_type(); }
+  
+  const Eigen::Vector3f position() const {
+    return Eigen::Map<const Eigen::Vector3f>(msg_.position().data());
+  }
+  const Eigen::Vector3f velocity() const{
+    return Eigen::Map<const Eigen::Vector3f>(msg_.velocity().data());
+  }
+};
+
+class MotorStates : public SubscriptionBase<unitree_go::msg::dds_::MotorStates_>
+{
+public:
+  using SharedPtr = std::shared_ptr<unitree_go::msg::dds_::MotorStates_>;
+
+  MotorStates(std::string topic, int num = 0) : SubscriptionBase<MsgType>(topic) 
+  {
+    if (num != 0) msg_.states().resize(num);
+  }
+};
+
+class MotorCmds : public SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>
+{
+public:
+  using SharedPtr = std::shared_ptr<unitree_go::msg::dds_::MotorCmds_>;
+
+  MotorCmds(std::string topic, int num = 0) : SubscriptionBase<MsgType>(topic) 
+  {
+    if (num != 0) msg_.cmds().resize(num);
+  }
+};
+
+
+} // namespace subscriber
+} // namespace go2
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/r1/defines.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <array>
+
+namespace unitree
+{
+namespace robot
+{
+namespace r1
+{
+
+enum class JointIndex
+{
+    // left leg
+    LeftHipPitch = 0,
+    LeftHipRoll,
+    LeftHipYaw,
+    LeftKnee,
+    LeftAnklePitch,
+    LeftAnkleRoll,
+
+    // right leg
+    RightHipPitch,
+    RightHipRoll,
+    RightHipYaw,
+    RightKnee,
+    RightAnklePitch,
+    RightAnkleRoll,
+
+    WaistRoll,
+    WaistYaw,
+    // no pitch
+
+    // left arm
+    LeftShoulderPitch = 15,
+    LeftShoulderRoll,
+    LeftShoulderYaw,
+    LeftElbow,
+    LeftWristRoll,
+
+    // right arm
+    RightShoulderPitch = 22,
+    RightShoulderRoll,
+    RightShoulderYaw,
+    RightElbow,
+    RightWristRoll,
+
+    // head
+    HeadPitch = 29,
+    HeadYaw,
+};
+
+
+} // namespace r1
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/r1/r1.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "unitree/dds_wrapper/robots/r1/r1_pub.h"
+
+namespace unitree
+{
+namespace robot
+{
+namespace r1
+{
+
+
+} // namespace r1
+} // namespace robot
+} // namespace unitree

unitree_sdk2/include/unitree/dds_wrapper/robots/r1/r1_pub.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include <unitree/dds_wrapper/common/Publisher.h>
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include "defines.h"
+
+namespace unitree
+{
+namespace robot
+{
+namespace r1
+{
+namespace publisher
+{
+
+class ArmSdk : public RealTimePublisher<unitree_hg::msg::dds_::LowCmd_>
+{
+public:
+    ArmSdk(std::string topic = "rt/arm_sdk") : RealTimePublisher<MsgType>(topic) 
+    {
+    }
+
+    void weight(float coe) { msg_.mode_pr() = std::clamp(int(coe * 100.0f), 0, 100); }
+
+    float weight() const { return float(msg_.mode_pr()) / 100.0; }
+
+    static constexpr std::array<JointIndex, 13> JOINTS = {
+        JointIndex::LeftShoulderPitch, 
+        JointIndex::LeftShoulderRoll,
+        JointIndex::LeftShoulderYaw,   
+        JointIndex::LeftElbow,
+        JointIndex::LeftWristRoll,     
+        JointIndex::RightShoulderPitch,
+        JointIndex::RightShoulderRoll, 
+        JointIndex::RightShoulderYaw,
+        JointIndex::RightElbow,        
+        JointIndex::RightWristRoll,
+        JointIndex::WaistYaw,          
+        JointIndex::HeadPitch,
+        JointIndex::HeadYaw,
+    };
+};
+
+}
+}
+}
+}