初始化项目，包含fast_lio_ws和unitree_sdk2

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