初始化项目，包含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