初始化项目，包含fast_lio_ws和unitree_sdk2

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