add unitree_dds_wrapper

Author: Agnel-Wang

README.md

@@ -18,7 +18,7 @@ You can install the required packages on Ubuntu 20.04 with:
 
 ```bash
 apt-get update
-apt-get install -y cmake g++ build-essential libyaml-cpp-dev libeigen3-dev
+apt-get install -y cmake g++ build-essential libyaml-cpp-dev libeigen3-dev libboost-all-dev libspdlog-dev libfmt-dev
 ```
 
 ### Build examples

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

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>
+#include <spdlog/spdlog.h>
+
+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;
+        spdlog::warn("Waiting for connection {}", sub_->GetChannelName());
+      }
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100)); // wait for stable communicaiton
+    if (warn_info) {
+      spdlog::info("Connected {}", sub_->GetChannelName());
+    }
+  }
+
+  MessageType msg_;
+protected:
+  virtual void pre_communication() {}  // something before receiving message
+  virtual void post_communication() {} // something after receiving message
+
+  uint32_t timeout_ms_{1000};
+  std::mutex mutex_;
+  unitree::robot::ChannelSubscriberPtr<MessageType> sub_;
+  std::chrono::steady_clock::time_point last_update_time_;
+};
+
+
+}; // namespace robot
+}; // namespace unitree

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;
+}