抽象力控

.vscode/settings.json

@@ -1,5 +1,9 @@
 {
     "files.associations": {
-        "functional": "cpp"
+        "deque": "cpp",
+        "string": "cpp",
+        "vector": "cpp",
+        "chrono": "cpp",
+        "array": "cpp"
     }
 }

README.md

@@ -2,3 +2,4 @@
 
 A simple ros2 program for legged robot . Robocon2025
 
+有点不太会用ros2 controller 所以直接做的控制。

build.sh

@@ -0,0 +1,2 @@
+source install/setup.bash
+colcon build

src/commands/joystick_input/src/JoystickInput.cpp

@@ -13,30 +13,28 @@ JoystickInput::JoystickInput() : Node("joysick_input_node") {
 }
 
 void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
-    if (msg->buttons[1] && msg->buttons[4]) {
+    if (msg->buttons[0]) {
-        inputs_.command = 1; // LB + B
+        inputs_.command = 1; // BACK
-    } else if (msg->buttons[0] && msg->buttons[4]) {
+    } else if (msg->buttons[1]) {
-        inputs_.command = 2; // LB + A
+        inputs_.command = 2; // RB
-    } else if (msg->buttons[2] && msg->buttons[4]) {
+    } else if (msg->buttons[3]) {
-        inputs_.command = 3; // LB + X
+        inputs_.command = 3; // LB
-    } else if (msg->buttons[3] && msg->buttons[4]) {
+    } else if (msg->buttons[4]) {
-        inputs_.command = 4; // LB + Y
+        inputs_.command = 4; // Y
-    } else if (msg->axes[2] != 1 && msg->buttons[1]) {
+    } else if (msg->buttons[6]) {
-        inputs_.command = 5; // LT + B
+        inputs_.command = 5; // X
-    } else if (msg->axes[2] != 1 && msg->buttons[0]) {
-        inputs_.command = 6; // LT + A
-    } else if (msg->axes[2] != 1 && msg->buttons[2]) {
-        inputs_.command = 7; // LT + X
-    } else if (msg->axes[2] != 1 && msg->buttons[3]) {
-        inputs_.command = 8; // LT + Y
     } else if (msg->buttons[7]) {
-        inputs_.command = 9; // START
+        inputs_.command = 6; // B
+    } else if (msg->buttons[8]) {
+        inputs_.command = 7; // A
+    } else if (msg->buttons[9]) {
+         inputs_.command = 8;
     } else {
         inputs_.command = 0;
         inputs_.lx = -msg->axes[0];
         inputs_.ly = msg->axes[1];
-        inputs_.rx = -msg->axes[3];
+        inputs_.rx = -msg->axes[2];
-        inputs_.ry = msg->axes[4];
+        inputs_.ry = msg->axes[3];
     }
     publisher_->publish(inputs_);
 }

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.8)
+project(mdog_simple_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rc_msgs REQUIRED)
+find_package(control_input_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+
+add_executable(mdog_simple_controller
+  src/mdog_simple_controller.cpp
+  src/FSM/state_safe.cpp
+  src/FSM/state_stand.cpp
+  src/FSM/state_zero.cpp
+  src/FSM/state_walk.cpp
+  src/FSM/state_balance.cpp
+  src/FSM/state_troting.cpp
+  src/common/kinematics.cpp
+  src/common/user_math.cpp
+  src/common/bezier_curve.cpp
+  src/common/pid.cpp
+)
+ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs )
+
+# add_executable(mdog_sim_controller
+#   src/mdog_sim_controller.cpp
+#   src/FSM/state_safe.cpp
+#   src/FSM/state_zero.cpp
+#   src/FSM/state_balance.cpp
+#   src/FSM/state_troting.cpp
+#   src/common/kinematics.cpp
+#   src/common/user_math.cpp
+#   src/common/bezier_curve.cpp
+# )
+# ament_target_dependencies(mdog_sim_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs)
+
+install(TARGETS
+  mdog_simple_controller
+  # mdog_sim_controller
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/fsm_state.hpp

@@ -0,0 +1,15 @@
+#pragma once
+
+namespace mdog_simple_controller {
+
+class MdogSimpleController;
+
+class FSMState {
+public:
+    virtual ~FSMState() = default;
+    virtual void enter(MdogSimpleController*) {}
+    virtual void run(MdogSimpleController*) = 0;
+    virtual void exit(MdogSimpleController*) {}
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateBalance : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_safe.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateSafe : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp

@@ -0,0 +1,40 @@
+#pragma once
+#include <array>
+#include <cstdint>
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateStand : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+
+private:
+    bool inited = false;
+    bool if_stand = false;
+    uint16_t joint_reached = 0;
+    float max_speed = 0.001;
+    float pos_threshold = 0.01;
+    float speed_threshold = 0.01;
+    // 默认PD参数
+    std::array<double, 3> kp = {4.5, 4.5, 4.5};
+    std::array<double, 3> kd = {0.05, 0.05, 0.05};
+
+    std::array<std::array<double, 3>, 4> stand_foot_pos = {
+        {
+            {0.0, 0.05, -0.25}, // FR
+            {0.0, 0.05, -0.25}, // FL
+            {0.0, 0.05, -0.25}, // RR
+            {0.0, 0.05, -0.25}  // RL
+        }
+    };
+
+    std::array<std::array<double, 3>, 4> target_joint_pos;
+    std::array<std::array<double, 3>, 4> start_joint_pos;
+    std::array<std::array<double, 3>, 4> last_joint_pos;
+
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_troting.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateTroting : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_walk.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateWalk : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_zero.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateZero : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/bezier_curve.hpp

@@ -0,0 +1,18 @@
+#pragma once
+#include <vector>
+#include <array>
+
+namespace mdog_simple_controller {
+namespace bezier {
+
+// 一维贝塞尔曲线
+double bezier_curve_1d(const std::vector<double>& control_points, double t);
+
+// 二维贝塞尔曲线
+std::array<double, 2> bezier_curve_2d(const std::vector<std::array<double, 2>>& control_points, double t);
+
+// 三维贝塞尔曲线
+std::array<double, 3> bezier_curve_3d(const std::vector<std::array<double, 3>>& control_points, double t);
+
+} // namespace bezier
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp

@@ -0,0 +1,69 @@
+#pragma once
+#include <array>
+
+namespace mdog_simple_controller {
+namespace kinematics {
+
+// 固定三连杆长度（单位：米，可根据实际机器人参数修改）
+constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.08, 0.25, 0.25};
+
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length();
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs();
+
+// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL）
+const std::array<std::array<double, 3>, 4>& get_hip_offsets();
+
+
+// 正运动学：已知关节角，求末端位置
+void forward_kinematics(const std::array<double, 3>& theta,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& pos);
+
+// 逆运动学：已知末端位置，求关节角。返回值为是否可达
+bool inverse_kinematics(const std::array<double, 3>& pos,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& theta);
+
+// 躯干->单腿坐标变换
+void foot_body_to_leg(const std::array<double, 3>& foot_body,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_leg);
+
+// 单腿->躯干坐标变换
+void foot_leg_to_body(const std::array<double, 3>& foot_leg,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_body);
+
+// 躯干姿态变换（欧拉角+高度） body->world
+void body_to_world(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_body,
+                   std::array<double, 3>& foot_world);
+
+// world->body
+void world_to_body(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_world,
+                   std::array<double, 3>& foot_body);
+
+// 综合正运动学（关节角->世界坐标）
+void leg_forward_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& theta,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& foot_world);
+
+// 综合逆运动学（世界坐标->关节角）
+bool leg_inverse_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& foot_world,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& theta);
+                                  
+} // namespace kinematics
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+namespace mdog_simple_controller {
+
+struct PIDParam {
+    double kp{0.0};
+    double ki{0.0};
+    double kd{0.0};
+    double dt{0.01};
+    double max_out{1e6};
+    double min_out{-1e6};
+};
+
+class PID {
+public:
+    PID(const PIDParam& param);
+
+    void set_param(const PIDParam& param);
+    void reset();
+    double compute(double setpoint, double measurement);
+
+    const PIDParam& get_param() const { return param_; }
+
+private:
+    PIDParam param_;
+    double prev_error_{0.0};
+    double integral_{0.0};
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+// 关节0点位置offset和减速比定义
+constexpr float JOINT_OFFSET[4][3] = {
+    {-5.149f, -5.8456f, 21.230f}, // 可根据实际填写
+    {-3.156f, 0.0f, 0.0f},
+    {0.0f, 0.0f, 0.0f},
+    // {0.2f, 0.393f, 0.5507f}
+    {5.63f, 2.9f, 0.0f}
+};
+
+constexpr float JOINT_RATIO[4][3] = {
+    {6.33f, 6.33f, 6.33f},
+    {6.33f, 6.33f, 6.33f},
+    {6.33f, 6.33f, 6.33f},
+    {6.33f, 6.33f, 6.33f}
+};
+
+// feedback → real
+void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst);
+
+// real → feedback
+void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst);
+
+// real cmd → 输出 cmd
+void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst);
+
+void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd);
+
+double limit_speed(double current, double target, double max_speed);
+int8_t get_direction(double target, double current);
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include "rc_msgs/msg/leg_fdb.hpp"
+#include "rc_msgs/msg/leg_cmd.hpp"
+#include "control_input_msgs/msg/inputs.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "geometry_msgs/msg/vector3.hpp"
+#include "mdog_simple_controller/FSM/state_safe.hpp"
+#include "mdog_simple_controller/FSM/state_stand.hpp"
+#include "mdog_simple_controller/FSM/state_zero.hpp"
+#include "mdog_simple_controller/FSM/state_walk.hpp"
+#include "mdog_simple_controller/FSM/state_balance.hpp"
+#include "mdog_simple_controller/FSM/state_troting.hpp"
+
+
+namespace mdog_simple_controller
+{
+
+struct LegJointData {
+    float torque[3];
+    float speed[3];
+    float pos[3];
+};
+
+struct LegJointCmd {
+    float torque_des[3];
+    float speed_des[3];
+    float pos_des[3];
+    float kp[3];
+    float kd[3];
+};
+
+struct Voltage{
+    float vx;
+    float vy;
+    float wz;
+};
+
+struct AHRS_Eulr_t {
+    float yaw;
+    float rol;
+    float pit;
+};
+
+struct InputCmd {
+    Voltage voltage;
+    AHRS_Eulr_t eulr;
+    float hight;
+    int8_t status;
+};
+
+struct MdogControllerData {
+    LegJointData feedback[4];
+    LegJointData feedback_real[4];
+    LegJointCmd command[4];
+    LegJointCmd command_real[4];
+    AHRS_Eulr_t imu_eulr;
+    AHRS_Eulr_t cmd_eulr;
+};
+
+
+class MdogSimpleController : public rclcpp::Node {
+public:
+    MdogSimpleController();
+
+    void change_state(FSMState* new_state);
+
+    MdogControllerData data_;
+    InputCmd input_cmd_;
+
+    bool if_no_hardware_{false};
+    bool if_debug_{false};
+private:
+    void fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg);
+    void input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg);
+    void ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg);
+    void control_loop();
+
+    FSMState* current_state_{nullptr};
+    StateSafe state_safe_;
+    StateStand state_stand_;
+    StateZero state_zero_;
+    StateWalk state_walk_;
+    StateBalance state_balance_;
+    StateTroting state_troting_;
+
+
+
+    rclcpp::Subscription<rc_msgs::msg::LegFdb>::SharedPtr fdb_sub_;
+    rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr input_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr ahrs_sub_;
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
+
+    rclcpp::Publisher<rc_msgs::msg::LegCmd>::SharedPtr cmd_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mdog_simple_controller',
+            executable='mdog_simple_controller',
+            name='mdog_simple_controller',
+            output='screen',
+            parameters=[{'if_no_hardware': False}]
+        )
+    ])

src/controllers/mdog_simple_controller/launch/no_hardware.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mdog_simple_controller',
+            executable='mdog_simple_controller',
+            name='mdog_simple_controller',
+            output='screen',
+            parameters=[{'if_no_hardware': True}]
+        )
+    ])

src/controllers/mdog_simple_controller/package.xml

@@ -1,28 +1,22 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>mdog_hardware</name>
+  <name>mdog_simple_controller</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="1683502971@qq.com">robofish</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>hardware_interface</depend>
-
-  <depend>pluginlib</depend>
-
-  <depend>rclcpp_lifecycle</depend>
-
   <depend>rclcpp</depend>
+  <depend>rc_msgs</depend>
+  <depend>control_input_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-  <test_depend>ament_cmake_gmock</test_depend>
-  <test_depend>ros2_control_test_assets</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp

@@ -0,0 +1,68 @@
+#include "mdog_simple_controller/FSM/state_balance.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+
+void StateBalance::enter(MdogSimpleController* ctrl) {
+    // 进入BALANCE状态时的初始化
+}
+
+void StateBalance::run(MdogSimpleController* ctrl) {
+    using namespace kinematics;
+    const auto& link = get_leg_link_length();
+    const auto& hip_signs = get_hip_signs();
+
+    // 获取当前躯干高度和欧拉角
+    double height = ctrl->input_cmd_.hight > 0.1 ? ctrl->input_cmd_.hight : 0.35;
+    std::array<double, 3> body_pos = {0.0, 0.0, height};
+    std::array<double, 3> eulr = {
+        ctrl->data_.imu_eulr.rol,
+        ctrl->data_.imu_eulr.pit,
+        ctrl->data_.imu_eulr.yaw
+    };
+
+    // 原地平衡：足端目标点固定在body系下
+    std::array<std::array<double, 3>, 4> foot_body_targets;
+    for (int leg = 0; leg < 4; ++leg) {
+        // 可加扰动补偿：如roll/pitch影响下的z修正
+        double dz = 0.0;
+        // dz = -body_pos[2] * std::sin(eulr[0]); // roll补偿示例
+        foot_body_targets[leg] = {0.0, 0.0, -height + dz};
+    }
+
+    // 计算足端世界坐标
+    std::array<std::array<double, 3>, 4> foot_world_targets;
+    for (int leg = 0; leg < 4; ++leg) {
+        std::array<double, 3> foot_leg;
+        foot_body_to_leg(foot_body_targets[leg], get_hip_offsets()[leg], foot_leg);
+        body_to_world(body_pos, eulr, foot_leg, foot_world_targets[leg]);
+    }
+
+    // 逆运动学求解每条腿的关节角
+    for (int leg = 0; leg < 4; ++leg) {
+        std::array<double, 3> theta;
+        bool ok = leg_inverse_kinematics_world(
+            leg, foot_world_targets[leg], link, body_pos, eulr, theta);
+        if (ok) {
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 1.0;
+                ctrl->data_.command_real[leg].kd[j] = 0.02;
+            }
+        }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateBalance::exit(MdogSimpleController* ctrl) {
+    // 离开BALANCE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_safe.cpp

@@ -0,0 +1,27 @@
+#include "mdog_simple_controller/FSM/state_safe.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+void StateSafe::enter(MdogSimpleController* ctrl) {
+    // 清空电机命令
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            ctrl->data_.command[leg].torque_des[j] = 0;
+            ctrl->data_.command[leg].speed_des[j] = 0;
+            ctrl->data_.command[leg].pos_des[j] = 0;
+            ctrl->data_.command[leg].kp[j] = 0;
+            ctrl->data_.command[leg].kd[j] = 0.05;
+        }
+    }
+}
+
+void StateSafe::run(MdogSimpleController* ctrl) {
+    // SAFE状态下的控制逻辑
+}
+
+void StateSafe::exit(MdogSimpleController* ctrl) {
+    // 离开SAFE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp

@@ -0,0 +1,69 @@
+#include "mdog_simple_controller/FSM/state_stand.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+namespace mdog_simple_controller {
+
+void StateStand::enter(MdogSimpleController* ctrl) {
+    inited = false;
+    if_stand = false;
+    max_speed = 0.04;
+    joint_reached = 0;
+    pos_threshold = 0.01;
+    speed_threshold = 0.01;
+
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    for (int leg = 0; leg < 4; ++leg) {
+        kinematics::inverse_kinematics(stand_foot_pos[leg], kinematics::get_leg_link_length(), target_joint_pos[leg]);
+        for (int j = 0; j < 3; ++j) {
+            start_joint_pos[leg][j] = ctrl->data_.feedback_real[leg].pos[j];
+            last_joint_pos[leg][j] = start_joint_pos[leg][j];
+            ctrl->data_.command_real[leg].pos_des[j] = 0;
+            ctrl->data_.command_real[leg].speed_des[j] = 0;
+            ctrl->data_.command_real[leg].torque_des[j] = 0;
+            ctrl->data_.command_real[leg].kp[j] = kp[j];
+            ctrl->data_.command_real[leg].kd[j] = kd[j];
+
+
+        }
+        target_joint_pos[leg][0] *= hip_signs[leg];
+    }
+
+}
+
+void StateStand::run(MdogSimpleController* ctrl) {
+
+    if (!if_stand) {
+        for (int leg = 0; leg < 4; ++leg) {
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = limit_speed(ctrl->data_.feedback_real[leg].pos[j], target_joint_pos[leg][j], max_speed);
+                if (!ctrl->if_no_hardware_){
+                    if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) > pos_threshold) {
+                        if (std::abs(ctrl->data_.feedback_real[leg].speed[j]) < speed_threshold) {
+                            ctrl->data_.command_real[leg].torque_des[j] += 0.01*get_direction(target_joint_pos[leg][j], ctrl->data_.feedback_real[leg].pos[j]);
+                            RCLCPP_INFO(ctrl->get_logger(), "leg %d joint %d torque_des: %f", leg, j, ctrl->data_.command_real[leg].torque_des[j]);
+
+                    } else {
+                        ctrl->data_.command_real[leg].speed_des[j] = 0;
+                    }
+                }
+                if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) < 0.01) {
+                    //将对应标志位置为1
+                    joint_reached |= (1 << (leg * 3 + j));
+                }
+            }
+        }
+        if (joint_reached == 0b0000111111111111) {
+            if_stand = true;
+        }
+    }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateStand::exit(MdogSimpleController* ctrl) {
+    // 离开SAFE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp

@@ -0,0 +1,88 @@
+#include "mdog_simple_controller/FSM/state_troting.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 1.2;
+}
+
+void StateTroting::enter(MdogSimpleController* ctrl) {
+    // 进入TROTING状态时的初始化
+}
+
+void StateTroting::run(MdogSimpleController* ctrl) {
+    //设置所有cmd数据为0
+    std::vector<std::array<double, 3>> control_points = {
+        {0.25, 0.0, -0.35},      // 起点
+        {0.1, 0.0, -0.25},     // 抬腿中点
+        {-0.05, 0.0, -0.35},     // 落点
+        // {0.40, 0.0, 0.05},      // 起点
+        // {0.40, 0.0, 0.05},     // 抬腿中点
+
+        // {0.40, 0.0, 0.05},     // 抬腿中点
+        // {0.40, 0.0, 0.05},     // 抬腿中点
+        // {0.40, 0.0, 0.05},     // 落点
+    };
+    // 起点和终点
+    const auto& p0 = control_points.front();
+    const auto& p1 = control_points.back();
+
+    // 时间推进
+    static rclcpp::Time last_time = ctrl->now();
+    rclcpp::Time now = ctrl->now();
+    double dt = (now - last_time).seconds();
+    last_time = now;
+    bezier_time += dt;
+    if (bezier_time > bezier_period) bezier_time -= bezier_period;
+    double t = bezier_time / bezier_period; // [0,1]
+
+    const auto& link = kinematics::get_leg_link_length();
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    // 对角腿同步，交错运动，相位差0.5
+    for (int leg = 0; leg < 4; ++leg) {
+        // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
+        double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5;
+        double phase = std::fmod(t + phase_offset, 1.0);
+
+        std::array<double, 3> foot_pos;
+        if (phase < 0.5) {
+            // 摆动相：贝塞尔曲线
+            double swing_t = phase / 0.5; // 归一化到[0,1]
+            foot_pos = bezier::bezier_curve_3d(control_points, swing_t);
+        } else {
+            // 支撑相：直线连接首尾
+            double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1]
+            for (int i = 0; i < 3; ++i) {
+                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t;
+            }
+        }
+
+        std::array<double, 3> theta;
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            // 使用参数化的hip方向
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 1.2;
+                ctrl->data_.command_real[leg].kd[j] = 0.02;
+            }
+        }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateTroting::exit(MdogSimpleController* ctrl) {
+    // 离开TROTING状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp

@@ -0,0 +1,127 @@
+#include "mdog_simple_controller/FSM/state_walk.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+// ...existing code...
+namespace mdog_simple_controller 
+{
+
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 2;
+constexpr double swing_ratio = 0.25; // 摆动相占整个周期的比例
+const double phase_offset[4] = {0.0, 0.25, 0.5, 0.75}; // 四条腿交错
+
+// 新增：回位阶段相关变量
+bool homing_done = false;
+double homing_time = 0.0;
+constexpr double homing_duration = 3.0; // 回位持续3秒
+}
+
+void StateWalk::enter(MdogSimpleController* ctrl) {
+    homing_done = false;
+    homing_time = 0.0;
+    bezier_time = 0.0;
+    // 记录进入回位时每条腿的当前位置
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 保存初始关节角度
+            ctrl->data_.command_real[leg].pos_des[j] = ctrl->data_.feedback_real[leg].pos[j];
+        }
+    }
+}
+
+void StateWalk::run(MdogSimpleController* ctrl) {
+    static rclcpp::Time last_time = ctrl->now();
+    rclcpp::Time now = ctrl->now();
+    double dt = (now - last_time).seconds();
+    last_time = now;
+
+    if (!homing_done) {
+        homing_time += dt;
+        double alpha = std::min(homing_time / homing_duration, 1.0); // 插值系数
+
+        std::array<double, 3> home_pos = {0.0, 0.0, -0.35};
+        const auto& link = kinematics::get_leg_link_length();
+        const auto& hip_signs = kinematics::get_hip_signs();
+
+        std::array<double, 3> target_theta;
+        kinematics::inverse_kinematics(home_pos, link, target_theta);
+
+        for (int leg = 0; leg < 4; ++leg) {
+            std::array<double, 3> theta;
+            for (int j = 0; j < 3; ++j) {
+                // 每次都用当前位置做起点
+                double start = ctrl->data_.feedback_real[leg].pos[j];
+                double tgt = (j == 0) ? (target_theta[j] * hip_signs[leg]) : target_theta[j];
+                theta[j] = (1 - alpha) * start + alpha * tgt;
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 0.8;
+                ctrl->data_.command_real[leg].kd[j] = 0.01;
+            }
+        }
+        realcmd_to_cmd(ctrl->data_, ctrl->data_);
+
+        if (homing_time >= homing_duration) {
+            homing_done = true;
+        }
+        return;
+    }
+
+    // 以下内容必须在 run 函数体内
+    std::vector<std::array<double, 3>> control_points = {
+        {0.25, 0.0, -0.35},      // 起点
+        {0.1, 0.0, -0.25},     // 抬腿中点
+        {-0.05, 0.0, -0.35},     // 落点
+    };
+    const auto& p0 = control_points.front();
+    const auto& p1 = control_points.back();
+
+    bezier_time += dt;
+    if (bezier_time > bezier_period) bezier_time -= bezier_period;
+    double t = bezier_time / bezier_period; // [0,1]
+
+    const auto& link = kinematics::get_leg_link_length();
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    for (int leg = 0; leg < 4; ++leg) {
+        double phase = std::fmod(t + phase_offset[leg], 1.0);
+        std::array<double, 3> foot_pos;
+        if (phase < swing_ratio) {
+            // 摆动相：贝塞尔曲线
+            double swing_phase = phase / swing_ratio; // 归一化到[0,1]
+            foot_pos = bezier::bezier_curve_3d(control_points, swing_phase);
+        } else {
+            // 支撑相：直线滑动
+            double stance_phase = (phase - swing_ratio) / (1.0 - swing_ratio); // 归一化到[0,1]
+            for (int i = 0; i < 3; ++i) {
+                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_phase;
+            }
+        }
+
+        std::array<double, 3> theta;
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 0.8;
+                ctrl->data_.command_real[leg].kd[j] = 0.01;
+            }
+        }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateWalk::exit(MdogSimpleController* ctrl) {
+    // 可选：状态退出时清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp

@@ -0,0 +1,27 @@
+#include "mdog_simple_controller/FSM/state_zero.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+void StateZero::enter(MdogSimpleController* ctrl) {
+    // 清空电机命令
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            ctrl->data_.command[leg].torque_des[j] = 0;
+            ctrl->data_.command[leg].speed_des[j] = 0;
+            ctrl->data_.command[leg].pos_des[j] = 0;
+            ctrl->data_.command[leg].kp[j] = 0;
+            ctrl->data_.command[leg].kd[j] = 0.05;
+        }
+    }
+}
+
+void StateZero::run(MdogSimpleController* ctrl) {
+    // ZERO状态下的控制逻辑
+}
+
+void StateZero::exit(MdogSimpleController* ctrl) {
+    // 离开ZERO状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/common/bezier_curve.cpp

@@ -0,0 +1,46 @@
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+namespace bezier {
+
+// 组合数
+static double binomial_coefficient(int n, int k) {
+    double res = 1.0;
+    for (int i = 1; i <= k; ++i) {
+        res *= (n - i + 1) / double(i);
+    }
+    return res;
+}
+
+double bezier_curve_1d(const std::vector<double>& control_points, double t) {
+    int n = control_points.size() - 1;
+    double result = 0.0;
+    for (int i = 0; i <= n; ++i) {
+        double binom = binomial_coefficient(n, i);
+        result += binom * std::pow(1 - t, n - i) * std::pow(t, i) * control_points[i];
+    }
+    return result;
+}
+
+std::array<double, 2> bezier_curve_2d(const std::vector<std::array<double, 2>>& control_points, double t) {
+    std::vector<double> x, y;
+    for (const auto& pt : control_points) {
+        x.push_back(pt[0]);
+        y.push_back(pt[1]);
+    }
+    return { bezier_curve_1d(x, t), bezier_curve_1d(y, t) };
+}
+
+std::array<double, 3> bezier_curve_3d(const std::vector<std::array<double, 3>>& control_points, double t) {
+    std::vector<double> x, y, z;
+    for (const auto& pt : control_points) {
+        x.push_back(pt[0]);
+        y.push_back(pt[1]);
+        z.push_back(pt[2]);
+    }
+    return { bezier_curve_1d(x, t), bezier_curve_1d(y, t), bezier_curve_1d(z, t) };
+}
+
+} // namespace bezier
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/kinematics.cpp

@@ -0,0 +1,164 @@
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+namespace kinematics {
+
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length() {
+    return LEG_LINK_LENGTH;
+}
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs() {
+    static constexpr std::array<int, 4> HIP_SIGNS = {1, -1, 1, -1};
+    return HIP_SIGNS;
+}
+
+// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL）
+const std::array<std::array<double, 3>, 4>& get_hip_offsets() {
+    // 以body中心为原点，x前后，y左右，z向上
+    static constexpr std::array<std::array<double, 3>, 4> HIP_OFFSETS = {
+        std::array<double, 3>{ 0.20, -0.10, 0.0 }, // FR
+        std::array<double, 3>{ 0.20,  0.10, 0.0 }, // FL
+        std::array<double, 3>{-0.20, -0.10, 0.0 }, // RR
+        std::array<double, 3>{-0.20,  0.10, 0.0 }  // RL
+    };
+    return HIP_OFFSETS;
+}
+
+
+// 正运动学：已知关节角，求末端位置
+void forward_kinematics(const std::array<double, 3>& theta,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& pos) {
+    // theta: [hip_yaw, hip_pitch, knee_pitch]
+    // link:  [hip_offset(y方向), thigh_length(x方向), shank_length(x方向)]
+    double t1 = theta[0], t2 = theta[1], t3 = theta[2];
+    double l1 = link[0], l2 = link[1], l3 = link[2];
+
+    // 与逆运动学严格对应
+    double x = (l2 * cos(t2) + l3 * cos(t2 + t3)) * cos(t1);
+    double y = l1 + (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1);
+    double z = l2 * sin(t2) + l3 * sin(t2 + t3);
+
+    // 逆运动学中 x = -pos[2], y = pos[1], z = pos[0]
+    // 所以正运动学输出应为 pos[0]=z, pos[1]=y, pos[2]=-x
+    pos[0] = z;
+    pos[1] = y;
+    pos[2] = -x;
+}
+
+// 逆运动学：已知末端位置，求关节角
+bool inverse_kinematics(const std::array<double, 3>& pos,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& theta) {
+    double x = -pos[2], y = pos[1], z = pos[0];
+    double l1 = link[0], l2 = link[1], l3 = link[2];
+
+    double y1 = y - l1;
+    double t1 = atan2(y1, x);
+
+    double d = sqrt(x * x + y1 * y1);
+    double z1 = z;
+
+    double D = (d * d + z1 * z1 - l2 * l2 - l3 * l3) / (2 * l2 * l3);
+    if (D < -1.0 || D > 1.0) return false;
+
+    double t3 = atan2(-sqrt(1 - D * D), D);
+    double k1 = l2 + l3 * cos(t3);
+    double k2 = l3 * sin(t3);
+    double t2 = atan2(z1, d) - atan2(k2, k1);
+
+    theta[0] = t1;
+    theta[1] = t2;
+    theta[2] = t3;
+    return true;
+}
+
+// 躯干->单腿坐标变换
+void foot_body_to_leg(const std::array<double, 3>& foot_body,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_leg) {
+    for (int i = 0; i < 3; ++i)
+        foot_leg[i] = foot_body[i] - hip_offset[i];
+}
+
+// 单腿->躯干坐标变换
+void foot_leg_to_body(const std::array<double, 3>& foot_leg,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_body) {
+    for (int i = 0; i < 3; ++i)
+        foot_body[i] = foot_leg[i] + hip_offset[i];
+}
+
+// 躯干->世界坐标变换
+void body_to_world(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_body,
+                   std::array<double, 3>& foot_world) {
+    double cr = cos(eulr[0]), sr = sin(eulr[0]);
+    double cp = cos(eulr[1]), sp = sin(eulr[1]);
+    double cy = cos(eulr[2]), sy = sin(eulr[2]);
+    double R[3][3] = {
+        {cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr},
+        {sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr},
+        {-sp,   cp*sr,            cp*cr}
+    };
+    for (int i = 0; i < 3; ++i) {
+        foot_world[i] = body_pos[i];
+        for (int j = 0; j < 3; ++j)
+            foot_world[i] += R[i][j] * foot_body[j];
+    }
+}
+
+// 世界->躯干坐标变换
+void world_to_body(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_world,
+                   std::array<double, 3>& foot_body) {
+    std::array<double, 3> delta;
+    for (int i = 0; i < 3; ++i)
+        delta[i] = foot_world[i] - body_pos[i];
+    double cr = cos(eulr[0]), sr = sin(eulr[0]);
+    double cp = cos(eulr[1]), sp = sin(eulr[1]);
+    double cy = cos(eulr[2]), sy = sin(eulr[2]);
+    double R[3][3] = {
+        {cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr},
+        {sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr},
+        {-sp,   cp*sr,            cp*cr}
+    };
+    for (int i = 0; i < 3; ++i) {
+        foot_body[i] = 0;
+        for (int j = 0; j < 3; ++j)
+            foot_body[i] += R[j][i] * delta[j];
+    }
+}
+
+// 综合正运动学（关节角->世界坐标）
+void leg_forward_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& theta,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& foot_world) {
+    std::array<double, 3> foot_leg, foot_body;
+    forward_kinematics(theta, link, foot_leg);
+    foot_leg_to_body(foot_leg, get_hip_offsets()[leg_index], foot_body);
+    body_to_world(body_pos, eulr, foot_body, foot_world);
+}
+
+bool leg_inverse_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& foot_world,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& theta) {
+    std::array<double, 3> foot_body, foot_leg;
+    world_to_body(body_pos, eulr, foot_world, foot_body);
+    foot_body_to_leg(foot_body, get_hip_offsets()[leg_index], foot_leg);
+    return inverse_kinematics(foot_leg, link, theta);
+}
+
+} // namespace kinematics
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/pid.cpp

@@ -0,0 +1,29 @@
+#include "mdog_simple_controller/common/pid.hpp"
+
+namespace mdog_simple_controller {
+
+PID::PID(const PIDParam& param)
+    : param_(param), prev_error_(0.0), integral_(0.0) {}
+
+void PID::set_param(const PIDParam& param) {
+    param_ = param;
+    reset();
+}
+
+void PID::reset() {
+    prev_error_ = 0.0;
+    integral_ = 0.0;
+}
+
+double PID::compute(double setpoint, double measurement) {
+    double error = setpoint - measurement;
+    integral_ += error * param_.dt;
+    double derivative = (error - prev_error_) / param_.dt;
+    double output = param_.kp * error + param_.ki * integral_ + param_.kd * derivative;
+    if (output > param_.max_out) output = param_.max_out;
+    if (output < param_.min_out) output = param_.min_out;
+    prev_error_ = error;
+    return output;
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/user_math.cpp

@@ -0,0 +1,86 @@
+#include "mdog_simple_controller/common/user_math.hpp"
+
+namespace mdog_simple_controller {
+
+// feedback → real
+void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：去掉offset，除以减速比
+            dst.feedback_real[leg].pos[j] = (src.feedback[leg].pos[j] - JOINT_OFFSET[leg][j]) / JOINT_RATIO[leg][j];
+            // 速度：除以减速比
+            dst.feedback_real[leg].speed[j] = src.feedback[leg].speed[j] / JOINT_RATIO[leg][j];
+            // 力矩：乘以减速比
+            dst.feedback_real[leg].torque[j] = src.feedback[leg].torque[j] * JOINT_RATIO[leg][j];
+        }
+    }
+}
+
+// real -> feedback
+void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：乘以减速比，加offset
+            dst.feedback[leg].pos[j] = src.feedback_real[leg].pos[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j];
+            // 速度：乘以减速比
+            dst.feedback[leg].speed[j] = src.feedback_real[leg].speed[j] * JOINT_RATIO[leg][j];
+            // 力矩：除以减速比
+            dst.feedback[leg].torque[j] = src.feedback_real[leg].torque[j] / JOINT_RATIO[leg][j];
+        }
+    }
+}
+
+// real cmd → 输出 cmd
+void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：乘以减速比，加offset
+            dst.command[leg].pos_des[j] = src.command_real[leg].pos_des[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j];
+            // 速度：乘以减速比
+            dst.command[leg].speed_des[j] = src.command_real[leg].speed_des[j] * JOINT_RATIO[leg][j];
+            // 力矩：除以减速比
+            dst.command[leg].torque_des[j] = src.command_real[leg].torque_des[j] / JOINT_RATIO[leg][j];
+            // kp/kd直接赋值
+            dst.command[leg].kp[j] = src.command_real[leg].kp[j];
+            dst.command[leg].kd[j] = src.command_real[leg].kd[j];
+        }
+    }
+}
+
+void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            data.command[leg].kp[j] = kp[j];
+            data.command[leg].kd[j] = kd[j];
+        }
+    }
+}
+
+//pd计算公式：
+// u =du + kp * (target - current) + kd * (target_speed - current_speed)
+
+// double simple_torque_ctrl(double detal_pos, )
+
+//计算方向
+int8_t get_direction(double target, double current) {
+    if (target > current) {
+        return 1;
+    } else if (target < current) {
+        return -1;
+    }
+    return 0;
+}
+
+// 移动速度最大限制器
+double limit_speed(double current, double target, double max_speed) {
+    double diff = target - current;
+    if (diff > max_speed) {
+        return current + max_speed;
+    } else if (diff < -max_speed) {
+        return current - max_speed;
+    }
+    return target;
+
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -0,0 +1,138 @@
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+
+namespace mdog_simple_controller
+{
+
+MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
+    fdb_sub_ = this->create_subscription<rc_msgs::msg::LegFdb>(
+        "/leg_fdb", 10,
+        std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1));
+    input_sub_ = this->create_subscription<control_input_msgs::msg::Inputs>(
+        "/control_input", 10,
+        std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1));
+    ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
+        "/eulr_angles", 10,
+        std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1));
+    cmd_pub_ = this->create_publisher<rc_msgs::msg::LegCmd>("/leg_cmd", 10);
+    joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
+
+    if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
+    if_debug_ = this->declare_parameter<bool>("if_debug", false);
+
+    memset(&data_, 0, sizeof(data_));
+    current_state_ = &state_zero_;
+    current_state_->enter(this);
+    timer_ = this->create_wall_timer(
+        std::chrono::microseconds(2000),
+        std::bind(&MdogSimpleController::control_loop, this));
+}
+
+void MdogSimpleController::change_state(FSMState* new_state) {
+    if (current_state_) current_state_->exit(this);
+    current_state_ = new_state;
+    if (current_state_) current_state_->enter(this);
+}
+
+void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            int idx = leg * 3 + j;
+            data_.feedback[leg].torque[j] = msg->leg_fdb[idx].torque;
+            data_.feedback[leg].speed[j]  = msg->leg_fdb[idx].speed;
+            data_.feedback[leg].pos[j]    = msg->leg_fdb[idx].pos;
+        }
+    }
+    feedback_to_real(data_, data_);
+}
+
+void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg){
+    input_cmd_.voltage.vx = msg->lx;
+    input_cmd_.voltage.vy = msg->ly;
+    input_cmd_.voltage.wz = msg->rx;
+    input_cmd_.hight = 0.35;
+    input_cmd_.status = msg->command;
+}
+
+void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
+    data_.imu_eulr.yaw = msg->z;
+    data_.imu_eulr.rol = msg->x;
+    data_.imu_eulr.pit = msg->y;
+}
+
+void MdogSimpleController::control_loop() {
+    FSMState* target_state = nullptr;
+    switch (input_cmd_.status) {
+        case 0: target_state = &state_safe_; break;
+        // case 5: target_state = &state_balance_; break;
+        // case 6: target_state = &state_troting_; break;
+        // case 7: target_state = &state_walk_; break;
+        case 8: target_state = &state_stand_; break;
+        default: target_state = &state_safe_; break;
+    }
+    if (target_state != current_state_) {
+        change_state(target_state);
+    }
+    if (current_state_) current_state_->run(this);
+
+    if (if_no_hardware_) {
+        for (int leg = 0; leg < 4; ++leg) {
+            for (int j = 0; j < 3; ++j) {
+                data_.feedback_real[leg].torque[j] = data_.command_real[leg].torque_des[j];
+                data_.feedback_real[leg].speed[j]  = data_.command_real[leg].speed_des[j];
+                data_.feedback_real[leg].pos[j]    = data_.command_real[leg].pos_des[j];
+                data_.feedback[leg].torque[j] = data_.command_real[leg].torque_des[j];
+                data_.feedback[leg].speed[j]  = data_.command_real[leg].speed_des[j];
+            }
+        }
+        real_to_feedback(data_, data_);
+    }
+
+    rc_msgs::msg::LegCmd msg;
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            msg.leg_cmd[leg * 3 + j].torque_des = data_.command[leg].torque_des[j];
+            msg.leg_cmd[leg * 3 + j].speed_des = data_.command[leg].speed_des[j];
+            msg.leg_cmd[leg * 3 + j].pos_des = data_.command[leg].pos_des[j];
+            msg.leg_cmd[leg * 3 + j].kp = data_.command[leg].kp[j];
+            msg.leg_cmd[leg * 3 + j].kd = data_.command[leg].kd[j];
+        }
+    }
+    msg.header.stamp = this->now();
+    msg.header.frame_id = "leg_cmd";
+    cmd_pub_->publish(msg);
+    //打印调试信息
+
+        // RCLCPP_INFO(this->get_logger(), "cmd: %f %f %f %f %f %f %f %f %f %f %f %f",
+        //     data_.command[0].pos_des[0], data_.command[0].pos_des[1], data_.command[0].pos_des[2],
+        //     data_.command[1].pos_des[0], data_.command[1].pos_des[1], data_.command[1].pos_des[2],
+        //     data_.command[2].pos_des[0], data_.command[2].pos_des[1], data_.command[2].pos_des[2],
+        //     data_.command[3].pos_des[0], data_.command[3].pos_des[1], data_.command[3].pos_des[2]);
+
+    sensor_msgs::msg::JointState js_msg;
+    js_msg.header.stamp = this->now();
+    js_msg.name = {
+        "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
+        "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
+        "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint",
+        "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint"
+    };
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            js_msg.position.push_back(data_.feedback_real[leg].pos[j]);
+            js_msg.velocity.push_back(data_.feedback_real[leg].speed[j]);
+            js_msg.effort.push_back(data_.feedback_real[leg].torque[j]);
+        }
+    }
+    joint_state_pub_->publish(js_msg);
+}
+
+
+} // namespace mdog_simple_controller
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<mdog_simple_controller::MdogSimpleController>());
+    rclcpp::shutdown();
+    return 0;
+}

src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h

@@ -87,7 +87,7 @@ private:
   //topic
   std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
   std::string latlon_topic="latlon";
-  std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
+  std::string Eulr_angles_topic="/Eulr_angles", Magnetic_topic="/Magnetic";
   std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
 
 
@@ -97,7 +97,7 @@ private:
 
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
   
-  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Eulr_angles_pub_;
   rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
 
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr  twist_pub_;

src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
             'imu_frame_id_':'gyro_link',
             'mag_pose_2d_topic':'/mag_pose_2d',
             'Magnetic_topic':'/magnetic',
-            'Euler_angles_topic':'/euler_angles',
+            'Eulr_angles_topic':'/eulr_angles',
             'gps_topic':'/gps/fix',
             'twist_topic':'/system_speed',
             'NED_odom_topic':'/NED_odometry',

src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp

@@ -24,8 +24,8 @@ ahrsBringup::ahrsBringup()
   this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
   this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
 
-  this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
+  this->declare_parameter<std::string>("Eulr_angles_topic","/eulr_angles");
-  this->get_parameter("Euler_angles_topic", Euler_angles_topic);
+  this->get_parameter("Eulr_angles_topic", Eulr_angles_topic);
 
   this->declare_parameter<std::string>("gps_topic","/gps/fix");
   this->get_parameter("gps_topic", gps_topic);
@@ -51,7 +51,7 @@ ahrsBringup::ahrsBringup()
   // pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
   // pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
   // pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
-  // pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
+  // pravite_nh.param("Eulr_angles_pub_", Eulr_angles_topic_, std::string("/eulr_angles"));
   // pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
  
   //serial                                                 
@@ -64,7 +64,7 @@ ahrsBringup::ahrsBringup()
 
   mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
 
-  Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
+  Eulr_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Eulr_angles_topic.c_str(), 10);
   Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
  
   twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
@@ -500,15 +500,15 @@ void ahrsBringup::processLoop()  // 数据处理过程
       pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
       mag_pose_pub_->publish(pose_2d);
       //std::cout << "YAW: " << pose_2d.theta << std::endl;
-      geometry_msgs::msg::Vector3 Euler_angles_2d,Magnetic;  
+      geometry_msgs::msg::Vector3 Eulr_angles_2d,Magnetic;  
-      Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
+      Eulr_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
-      Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
+      Eulr_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
-      Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
+      Eulr_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
       Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
       Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
       Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
 
-      Euler_angles_pub_->publish(Euler_angles_2d);
+      Eulr_angles_pub_->publish(Eulr_angles_2d);
       Magnetic_pub_->publish(Magnetic);
 
     }

src/hardware/mdog_hardware/CMakeLists.txt

@@ -1,93 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(mdog_hardware)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(hardware_interface REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-
-add_library(
-  mdog_hardware
-  SHARED
-  src/mdog_hardware_interface.cpp
-)
-target_include_directories(
-  mdog_hardware
-  PUBLIC
-  include
-)
-ament_target_dependencies(
-  mdog_hardware
-  hardware_interface
-  rclcpp
-  rclcpp_lifecycle
-)
-# prevent pluginlib from using boost
-target_compile_definitions(mdog_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
-
-pluginlib_export_plugin_description_file(
-  hardware_interface mdog_hardware.xml)
-
-install(
-  TARGETS
-  mdog_hardware
-  RUNTIME DESTINATION bin
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-)
-
-install(
-  DIRECTORY include/
-  DESTINATION include
-)
-
-install(
-  DIRECTORY launch
-  DESTINATION share/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-  find_package(ament_cmake_gmock REQUIRED)
-  find_package(ros2_control_test_assets REQUIRED)
-
-  ament_add_gmock(test_mdog_hardware_interface test/test_mdog_hardware_interface.cpp)
-  target_include_directories(test_mdog_hardware_interface PRIVATE include)
-  ament_target_dependencies(
-    test_mdog_hardware_interface
-    hardware_interface
-    pluginlib
-    ros2_control_test_assets
-  )
-endif()
-
-ament_export_include_directories(
-  include
-)
-ament_export_libraries(
-  mdog_hardware
-)
-ament_export_dependencies(
-  hardware_interface
-  pluginlib
-  rclcpp
-  rclcpp_lifecycle
-)
-
-ament_package()

src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp

@@ -1,79 +0,0 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
-#define MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
-
-#include <string>
-#include <vector>
-
-#include "mdog_hardware/visibility_control.h"
-#include "hardware_interface/system_interface.hpp"
-#include "hardware_interface/handle.hpp"
-#include "hardware_interface/hardware_info.hpp"
-#include "hardware_interface/types/hardware_interface_return_values.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp_lifecycle/state.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/imu.hpp"
-
-namespace mdog_hardware
-{
-class MDogHardwareInterface : public hardware_interface::SystemInterface
-{
-public:
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  hardware_interface::CallbackReturn on_init(
-    const hardware_interface::HardwareInfo & info) override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  hardware_interface::CallbackReturn on_configure(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  hardware_interface::CallbackReturn on_activate(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  hardware_interface::CallbackReturn on_deactivate(
-    const rclcpp_lifecycle::State & previous_state) override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  hardware_interface::return_type read(
-    const rclcpp::Time & time, const rclcpp::Duration & period) override;
-
-  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-  hardware_interface::return_type write(
-    const rclcpp::Time & time, const rclcpp::Duration & period) override;
-
-private:
-  std::vector<double> hw_commands_;
-  std::vector<double> hw_states_;
-  std::vector<double> imu_states_;
-
-  rclcpp::Node::SharedPtr node_;
-  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
-
-  void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
-};
-
-}  // namespace mdog_hardware
-
-#endif  // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_

src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h

@@ -1,49 +0,0 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MDOG_HARDWARE__VISIBILITY_CONTROL_H_
-#define MDOG_HARDWARE__VISIBILITY_CONTROL_H_
-
-// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
-//     https://gcc.gnu.org/wiki/Visibility
-
-#if defined _WIN32 || defined __CYGWIN__
-#ifdef __GNUC__
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport))
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport))
-#else
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport)
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport)
-#endif
-#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT
-#else
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
-#endif
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
-#else
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default")))
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
-#if __GNUC__ >= 4
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default")))
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
-#else
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
-#endif
-#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
-#endif
-
-#endif  // MDOG_HARDWARE__VISIBILITY_CONTROL_H_

src/hardware/mdog_hardware/launch/mdog_hardware.launch.py

@@ -1,43 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-import xacro  # 添加 xacro 模块
-
-
-def process_xacro():
-    # 获取 xacro 文件路径
-    pkg_path = os.path.join(get_package_share_directory('mdog_description'))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    # 处理 xacro 文件
-    robot_description_config = xacro.process_file(xacro_file)
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    # 生成机器人描述
-    robot_description = {'robot_description': process_xacro()}
-
-    # 启动机器人状态发布器
-    robot_state_publisher_node = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        output='screen',
-        parameters=[robot_description]
-    )
-
-    # 启动控制器管理器
-    controller_manager_node = Node(
-        package='controller_manager',
-        executable='ros2_control_node',
-        output='screen',
-        parameters=[robot_description],
-        arguments=['--ros-args', '--log-level', 'info']
-    )
-
-    return LaunchDescription([
-        robot_state_publisher_node,
-        controller_manager_node
-    ])

src/hardware/mdog_hardware/mdog_hardware.xml

@@ -1,9 +0,0 @@
-<library path="mdog_hardware">
-  <class name="mdog_hardware/MDogHardwareInterface"
-         type="mdog_hardware::MDogHardwareInterface"
-         base_class_type="hardware_interface::SystemInterface">
-    <description>
-      ros2_control hardware interface.
-    </description>
-  </class>
-</library>

src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp

@@ -1,165 +0,0 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <limits>
-#include <vector>
-
-#include "mdog_hardware/mdog_hardware_interface.hpp"
-#include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/imu.hpp"
-
-namespace mdog_hardware
-{
-
-void MDogHardwareInterface::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
-{
-  imu_states_[0] = msg->orientation.w;
-  imu_states_[1] = msg->orientation.x;
-  imu_states_[2] = msg->orientation.y;
-  imu_states_[3] = msg->orientation.z;
-
-  imu_states_[4] = msg->angular_velocity.x;
-  imu_states_[5] = msg->angular_velocity.y;
-  imu_states_[6] = msg->angular_velocity.z;
-
-  imu_states_[7] = msg->linear_acceleration.x;
-  imu_states_[8] = msg->linear_acceleration.y;
-  imu_states_[9] = msg->linear_acceleration.z;
-}
-
-hardware_interface::CallbackReturn MDogHardwareInterface::on_init(
-  const hardware_interface::HardwareInfo & info)
-{
-  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
-  {
-    return CallbackReturn::ERROR;
-  }
-
-  // 初始化硬件状态和命令
-  hw_states_.resize(info_.joints.size() * 3, std::numeric_limits<double>::quiet_NaN());
-  hw_commands_.resize(info_.joints.size() * 5, std::numeric_limits<double>::quiet_NaN());
-  imu_states_.resize(10, std::numeric_limits<double>::quiet_NaN());
-
-  // 创建 ROS 2 节点和订阅器
-  node_ = std::make_shared<rclcpp::Node>("mdog_hardware_interface");
-  imu_subscription_ = node_->create_subscription<sensor_msgs::msg::Imu>(
-    "/imu", 10, std::bind(&MDogHardwareInterface::imu_callback, this, std::placeholders::_1));
-
-  return CallbackReturn::SUCCESS;
-}
-
-hardware_interface::CallbackReturn MDogHardwareInterface::on_configure(
-  const rclcpp_lifecycle::State & /*previous_state*/)
-{
-  // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
-
-  return CallbackReturn::SUCCESS;
-}
-
-std::vector<hardware_interface::StateInterface> MDogHardwareInterface::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (size_t i = 0; i < info_.joints.size(); ++i)
-  {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i * 3]));
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_[i * 3 + 1]));
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_[i * 3 + 2]));
-    
-  }
-  
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "orientation.w", &imu_states_[0]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "orientation.x", &imu_states_[1]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "orientation.y", &imu_states_[2]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "orientation.z", &imu_states_[3]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "angular_velocity.x", &imu_states_[4]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "angular_velocity.y", &imu_states_[5]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "angular_velocity.z", &imu_states_[6]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "linear_acceleration.x", &imu_states_[7]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "linear_acceleration.y", &imu_states_[8]));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(
-    "imu_sensor", "linear_acceleration.z", &imu_states_[9]));
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface> MDogHardwareInterface::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-  for (size_t i = 0; i < info_.joints.size(); ++i)
-  {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i * 5]));
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i * 5 + 1]));
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i * 5 + 2]));
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, "kp", &hw_commands_[i * 5 + 3]));
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, "kd", &hw_commands_[i * 5 + 4]));
-  }
-  return command_interfaces;
-}
-
-hardware_interface::CallbackReturn MDogHardwareInterface::on_activate(
-  const rclcpp_lifecycle::State & /*previous_state*/)
-{
-  // TODO(anyone): prepare the robot to receive commands
-
-  return CallbackReturn::SUCCESS;
-}
-
-hardware_interface::CallbackReturn MDogHardwareInterface::on_deactivate(
-  const rclcpp_lifecycle::State & /*previous_state*/)
-{
-  // TODO(anyone): prepare the robot to stop receiving commands
-
-  return CallbackReturn::SUCCESS;
-}
-
-hardware_interface::return_type MDogHardwareInterface::read(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
-{
-  // TODO(anyone): read robot states
-
-  return hardware_interface::return_type::OK;
-}
-
-hardware_interface::return_type MDogHardwareInterface::write(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
-{
-  // TODO(anyone): write robot's commands'
-
-  return hardware_interface::return_type::OK;
-}
-
-}  // namespace mdog_hardware
-
-#include "pluginlib/class_list_macros.hpp"
-
-PLUGINLIB_EXPORT_CLASS(
-  mdog_hardware::MDogHardwareInterface, hardware_interface::SystemInterface)

src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp

@@ -1,57 +0,0 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gmock/gmock.h>
-
-#include <string>
-
-#include "hardware_interface/resource_manager.hpp"
-#include "ros2_control_test_assets/components_urdfs.hpp"
-#include "ros2_control_test_assets/descriptions.hpp"
-
-class TestMDogHardwareInterface : public ::testing::Test
-{
-protected:
-  void SetUp() override
-  {
-    // TODO(anyone): Extend this description to your robot
-    mdog_hardware_interface_2dof_ =
-      R"(
-        <ros2_control name="MDogHardwareInterface2dof" type="system">
-          <hardware>
-            <plugin>mdog_hardware/MDogHardwareInterface</plugin>
-          </hardware>
-          <joint name="joint1">
-            <command_interface name="position"/>
-            <state_interface name="position"/>
-            <param name="initial_position">1.57</param>
-          </joint>
-          <joint name="joint2">
-            <command_interface name="position"/>
-            <state_interface name="position"/>
-            <param name="initial_position">0.7854</param>
-          </joint>
-        </ros2_control>
-    )";
-  }
-
-  std::string mdog_hardware_interface_2dof_;
-};
-
-TEST_F(TestMDogHardwareInterface, load_mdog_hardware_interface_2dof)
-{
-  auto urdf = ros2_control_test_assets::urdf_head + mdog_hardware_interface_2dof_ +
-              ros2_control_test_assets::urdf_tail;
-  ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf));
-}

src/hardware/unitree_leg_serial_driver/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(serial REQUIRED)
 find_package(rc_msgs REQUIRED)
+
 # 添加头文件目录
 include_directories(include)
 

src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp

@@ -5,10 +5,11 @@
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <thread>
+#include <array>
 #include "unitree_leg_serial_driver/crc_ccitt.hpp"
 #include "unitree_leg_serial_driver/gom_protocol.hpp"
-#include "rc_msgs/msg/go_motor_cmd.hpp"
+#include "rc_msgs/msg/leg_fdb.hpp"
-#include "rc_msgs/msg/go_motor_fdb.hpp"
+#include "rc_msgs/msg/leg_cmd.hpp"
 namespace unitree_leg_serial
 {
 
@@ -18,41 +19,48 @@ public:
     explicit UnitreeLegSerial(const rclcpp::NodeOptions &options);
     ~UnitreeLegSerial() override;
 
-private:
+    void write(const std::array<MotorCmd_t, 12>& cmds);
-    void receive_data();
+    std::array<MotorData_t, 12> read();
-    void reopen_port();
-    void send_motor_data(const MotorCmd_t &cmd);
-    void motor_update();
-    void motor_cmd_reset();
-    bool open_serial_port();
-    void close_serial_port();
-    void motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg);
 
-    int send_count_;
+private:
-    int recv_count_;
+    static constexpr int PORT_NUM = 4;
+    static constexpr int MOTOR_NUM = 3;
+
+    void receive_data(int port_idx);
+    void reopen_port(int port_idx);
+    void send_motor_data(int port_idx, const MotorCmd_t &cmd);
+    void motor_update();
+    void motor_cmd_reset(int port_idx, int idx);
+    bool open_serial_port(int port_idx);
+    void close_serial_port(int port_idx);
+    void leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg);
+
+    int send_count_[PORT_NUM]{};
+    int recv_count_[PORT_NUM]{};
+    bool if_debug_{true};
     rclcpp::Time last_freq_time_;
 
-    rclcpp::Publisher<rc_msgs::msg::GoMotorFdb>::SharedPtr motor_fdb_pub_;
+    std::array<std::unique_ptr<serial::Serial>, PORT_NUM> serial_port_;
-    rclcpp::Subscription<rc_msgs::msg::GoMotorCmd>::SharedPtr motor_cmd_sub_;
-    
-
-    std::unique_ptr<serial::Serial> serial_port_;
     rclcpp::TimerBase::SharedPtr timer_;
-    std::thread read_thread_;
+    std::array<std::thread, PORT_NUM> read_thread_;
     std::atomic<bool> running_{false};
+    rclcpp::Publisher<rc_msgs::msg::LegFdb>::SharedPtr leg_fdb_pub_;
+    rclcpp::Subscription<rc_msgs::msg::LegCmd>::SharedPtr leg_cmd_sub_;
 
     // 状态管理
     enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 };
     std::atomic<StatusFlag> status_flag_{OFFLINE};
-    std::atomic<int8_t> tick_{0};
+    std::array<std::atomic<int8_t>, PORT_NUM> tick_;
 
     // 串口参数
-    std::string serial_port_name_;
+    std::array<std::string, PORT_NUM> serial_port_name_;
-    int baud_rate_;
+    std::array<int, PORT_NUM> baud_rate_;
 
     // 电机数据
-    MotorCmd_t motor_cmd_;
+    std::array<std::array<MotorCmd_t, MOTOR_NUM>, PORT_NUM> motor_cmd_;
-    MotorData_t motor_fbk_;
+    std::array<std::array<MotorData_t, MOTOR_NUM>, PORT_NUM> motor_fbk_;
+    std::array<int, PORT_NUM> current_motor_idx_{};
+    std::array<std::array<int, MOTOR_NUM>, PORT_NUM> send_id_count_{};
 };
 
 } // namespace unitree_leg_serial

src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py

@@ -1,9 +1,16 @@
 from launch import LaunchDescription
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 
 def generate_launch_description():
     return LaunchDescription([
+        DeclareLaunchArgument(
+            "if_debug",
+            default_value="true",
+            description="If true, use ROS topics for communication"
+        ),
         ComposableNodeContainer(
             name="component_container",
             namespace="",
@@ -13,7 +20,8 @@ def generate_launch_description():
                 ComposableNode(
                     package="unitree_leg_serial_driver",
                     plugin="unitree_leg_serial::UnitreeLegSerial",
-                    name="unitree_leg_serial"
+                    name="unitree_leg_serial",
+                    parameters=[{"if_debug": LaunchConfiguration("if_debug")}]
                 )
             ],
             output="screen"

src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp

@@ -1,120 +1,202 @@
 #include "unitree_leg_serial_driver/unitree_leg_serial.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
-#include "rc_msgs/msg/go_motor_cmd.hpp"
+#include "rc_msgs/msg/leg_fdb.hpp"
-#include "rc_msgs/msg/go_motor_fdb.hpp"
+#include "rc_msgs/msg/leg_cmd.hpp"
 namespace unitree_leg_serial
 {
 
 UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options)
     : Node("unitree_leg_serial", options)
 {
-    serial_port_name_ = "/dev/ttyACM0";
+    // 串口名和波特率初始化
-    baud_rate_ = 4000000;
+    serial_port_name_ = {"/dev/ttyACM0", "/dev/ttyACM1", "/dev/ttyACM2", "/dev/ttyACM3"};
+    baud_rate_ = {4000000, 4000000, 4000000, 4000000};
 
-    send_count_ = 0;
-    recv_count_ = 0;
     last_freq_time_ = this->now();
+    leg_fdb_pub_ = this->create_publisher<rc_msgs::msg::LegFdb>("leg_fdb", 10);
+    leg_cmd_sub_ = this->create_subscription<rc_msgs::msg::LegCmd>(
+        "leg_cmd", 10, std::bind(&UnitreeLegSerial::leg_cmd_callback, this, std::placeholders::_1));
 
-    // 发布器
+    for (int p = 0; p < PORT_NUM; ++p) {
-    motor_fdb_pub_ = this->create_publisher<rc_msgs::msg::GoMotorFdb>("motor_fdb", 10);
+        send_count_[p] = 0;
-    // 订阅器
+        recv_count_[p] = 0;
-    motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::GoMotorCmd>(
+        tick_[p] = 0;
-        "motor_cmd", 10,
+        current_motor_idx_[p] = 0;
-        std::bind(&UnitreeLegSerial::motor_cmd_callback, this, std::placeholders::_1));
+        send_id_count_[p].fill(0);
 
-    if (!open_serial_port()) {
+        // 初始化每个串口的3个电机命令
-        RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_.c_str());
+        for (int i = 0; i < MOTOR_NUM; ++i) {
-        return;
+            motor_cmd_[p][i] = MotorCmd_t{};
+            motor_cmd_[p][i].id = i;
+            motor_cmd_[p][i].mode = 1;
+        }
+
+        if (!open_serial_port(p)) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_[p].c_str());
+            continue;
+        }
     }
+    status_flag_ = OFFLINE;
 
     running_ = true;
-    status_flag_ = OFFLINE;
+    // 启动每个串口的接收线程
-    tick_ = 0;
+    for (int p = 0; p < PORT_NUM; ++p) {
-    read_thread_ = std::thread(&UnitreeLegSerial::receive_data, this);
+        if (serial_port_[p] && serial_port_[p]->isOpen()) {
+            read_thread_[p] = std::thread(&UnitreeLegSerial::receive_data, this, p);
+        }
+    }
 
+    // 定时器，轮询所有串口
     timer_ = this->create_wall_timer(
-        std::chrono::microseconds(500),
+        std::chrono::microseconds(333),
         [this]() { motor_update(); });
 }
 
 UnitreeLegSerial::~UnitreeLegSerial()
 {
     running_ = false;
-    if (read_thread_.joinable()) {
+    for (int p = 0; p < PORT_NUM; ++p) {
-        read_thread_.join();
+        if (read_thread_[p].joinable()) {
+            read_thread_[p].join();
+        }
+        close_serial_port(p);
     }
-    close_serial_port();
 }
 
-void UnitreeLegSerial::motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg)
+void UnitreeLegSerial::write(const std::array<MotorCmd_t, 12>& cmds)
 {
-    // 填充motor_cmd_结构体
+    for (int p = 0; p < PORT_NUM; ++p) {
-    motor_cmd_.T = msg->torque_des;
+        for (int m = 0; m < MOTOR_NUM; ++m) {
-    motor_cmd_.W = msg->speed_des;
+            int idx = p * MOTOR_NUM + m;
-    motor_cmd_.Pos = msg->pos_des;
+            if (idx < 12) {
-    motor_cmd_.K_P = msg->kp;
+                motor_cmd_[p][m] = cmds[idx];
-    motor_cmd_.K_W = msg->kd;
+            }
+        }
+    }
     status_flag_ = CONTROLED;
-    tick_ = 0;
 }
 
-bool UnitreeLegSerial::open_serial_port()
+std::array<MotorData_t, 12> UnitreeLegSerial::read()
+{
+    std::array<MotorData_t, 12> result;
+    for (int p = 0; p < PORT_NUM; ++p) {
+        for (int m = 0; m < MOTOR_NUM; ++m) {
+            int idx = p * MOTOR_NUM + m;
+            if (idx < 12) {
+                result[idx] = motor_fbk_[p][m];
+            }
+        }
+    }
+    return result;
+}
+
+void UnitreeLegSerial::leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg)
+{
+    std::array<MotorCmd_t, 12> cmds;
+    for (size_t i = 0; i < 12; ++i) {
+        cmds[i].T = msg->leg_cmd[i].torque_des;
+        cmds[i].W = msg->leg_cmd[i].speed_des;
+        cmds[i].Pos = msg->leg_cmd[i].pos_des;
+        cmds[i].K_P = msg->leg_cmd[i].kp;
+        cmds[i].K_W = msg->leg_cmd[i].kd;
+        cmds[i].id = i % 3;
+        cmds[i].mode = 1;
+    }
+    write(cmds);
+}
+
+bool UnitreeLegSerial::open_serial_port(int port_idx)
 {
     try {
-        serial_port_ = std::make_unique<serial::Serial>(serial_port_name_, baud_rate_, serial::Timeout::simpleTimeout(1000));
+        serial_port_[port_idx] = std::make_unique<serial::Serial>(
-        return serial_port_->isOpen();
+            serial_port_name_[port_idx], baud_rate_[port_idx], serial::Timeout::simpleTimeout(1000));
+        return serial_port_[port_idx]->isOpen();
     } catch (const std::exception &e) {
-        RCLCPP_ERROR(this->get_logger(), "Serial open exception: %s", e.what());
+        RCLCPP_ERROR(this->get_logger(), "Serial open exception [%d]: %s", port_idx, e.what());
         return false;
     }
 }
 
-void UnitreeLegSerial::close_serial_port()
+void UnitreeLegSerial::close_serial_port(int port_idx)
 {
-    if (serial_port_ && serial_port_->isOpen()) {
+    if (serial_port_[port_idx] && serial_port_[port_idx]->isOpen()) {
-        serial_port_->close();
+        serial_port_[port_idx]->close();
     }
 }
 
 void UnitreeLegSerial::motor_update()
 {
-    if (tick_ < 10) {
+    for (int p = 0; p < PORT_NUM; ++p) {
-        ++tick_;
+        if (tick_[p] < 500) {
-    } else {
+            ++tick_[p];
-        status_flag_ = OFFLINE;
+        } else {
+            status_flag_ = OFFLINE;
+        }
+
+        if (status_flag_ != CONTROLED) {
+            for (int i = 0; i < MOTOR_NUM; ++i) {
+                motor_cmd_reset(p, i);
+            }
+        }
+        // 每次只发送一个电机命令，轮流发送
+        send_motor_data(p, motor_cmd_[p][current_motor_idx_[p]]);
+        send_count_[p]++;
+        send_id_count_[p][current_motor_idx_[p]]++;
+        current_motor_idx_[p] = (current_motor_idx_[p] + 1) % MOTOR_NUM;
     }
 
-    if (status_flag_ != CONTROLED) {
+    // 每秒打印一次频率和所有电机状态
-        motor_cmd_reset();
-    }
-    send_motor_data(motor_cmd_);
-    send_count_++;
-
-    // 每秒打印一次频率
     auto now = this->now();
     if ((now - last_freq_time_).seconds() >= 1.0) {
-        RCLCPP_INFO(this->get_logger(), "发送频率: %d Hz, 接收频率: %d Hz", send_count_, recv_count_);
+        for (int p = 0; p < PORT_NUM; ++p) {
-        send_count_ = 0;
+            RCLCPP_INFO(this->get_logger(), "[Port%d] 发送频率: %d Hz, 接收频率: %d Hz", p, send_count_[p], recv_count_[p]);
-        recv_count_ = 0;
+            for (int i = 0; i < MOTOR_NUM; ++i) {
+                RCLCPP_INFO(this->get_logger(),
+                    "[Port%d] 电机%d: send_count=%d, Pos=%.3f, W=%.3f, T=%.3f, Temp=%d, Mode=%d, Correct=%d",
+                    p, i, send_id_count_[p][i],
+                    motor_fbk_[p][i].Pos,
+                    motor_fbk_[p][i].W,
+                    motor_fbk_[p][i].T,
+                    motor_fbk_[p][i].Temp,
+                    motor_fbk_[p][i].mode,
+                    motor_fbk_[p][i].correct
+                );
+                send_id_count_[p][i] = 0; // 重置
+            }
+            send_count_[p] = 0;
+            recv_count_[p] = 0;
+        }
         last_freq_time_ = now;
     }
+    rc_msgs::msg::LegFdb msg;
+
+    msg.header.stamp = this->now();
+    msg.header.frame_id = "leg"; // 可根据实际需要修改
+
+    auto fbk = read();
+    for (size_t i = 0; i < 12; ++i) {
+        msg.leg_fdb[i].torque = fbk[i].T;
+        msg.leg_fdb[i].speed = fbk[i].W;
+        msg.leg_fdb[i].pos = fbk[i].Pos;
+    }
+    leg_fdb_pub_->publish(msg);
 }
 
-void UnitreeLegSerial::motor_cmd_reset()
+void UnitreeLegSerial::motor_cmd_reset(int port_idx, int idx)
 {
-    motor_cmd_ = MotorCmd_t{};
+    motor_cmd_[port_idx][idx] = MotorCmd_t{};
-    motor_cmd_.id = 2;
+    motor_cmd_[port_idx][idx].id = idx;
-    motor_cmd_.mode = 1;
+    motor_cmd_[port_idx][idx].mode = 1;
 }
 
-void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd)
+void UnitreeLegSerial::send_motor_data(int port_idx, const MotorCmd_t &cmd)
 {
-    if (!serial_port_ || !serial_port_->isOpen()) {
+    if (!serial_port_[port_idx] || !serial_port_[port_idx]->isOpen()) {
-        RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Serial port is not open.");
+        RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[Port%d] Serial port is not open.", port_idx);
         return;
     }
 
-    auto &out = motor_cmd_.motor_send_data;
+    RIS_ControlData_t out;
     out.head[0] = 0xFE;
     out.head[1] = 0xEE;
 
@@ -142,10 +224,10 @@ void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd)
     out.CRC16 = crc_ccitt::crc_ccitt(
         0, (uint8_t *)&out, sizeof(RIS_ControlData_t) - sizeof(out.CRC16));
 
-    serial_port_->write(reinterpret_cast<const uint8_t *>(&out), sizeof(RIS_ControlData_t));
+    serial_port_[port_idx]->write(reinterpret_cast<const uint8_t *>(&out), sizeof(RIS_ControlData_t));
 }
 
-void UnitreeLegSerial::receive_data()
+void UnitreeLegSerial::receive_data(int port_idx)
 {
     size_t packet_size = sizeof(RIS_MotorData_t);
     std::vector<uint8_t> buffer(packet_size * 2);
@@ -153,7 +235,7 @@ void UnitreeLegSerial::receive_data()
 
     while (running_) {
         try {
-            size_t bytes_read = serial_port_->read(buffer.data() + buffer_offset, buffer.size() - buffer_offset);
+            size_t bytes_read = serial_port_[port_idx]->read(buffer.data() + buffer_offset, buffer.size() - buffer_offset);
             if (bytes_read == 0) continue;
             buffer_offset += bytes_read;
 
@@ -172,53 +254,57 @@ void UnitreeLegSerial::receive_data()
             }
             if (buffer_offset < packet_size) continue;
 
-            std::memcpy(&motor_fbk_.motor_recv_data, buffer.data(), packet_size);
+            RIS_MotorData_t recv_data;
+            std::memcpy(&recv_data, buffer.data(), packet_size);
 
-            if (motor_fbk_.motor_recv_data.head[0] != 0xFD || motor_fbk_.motor_recv_data.head[1] != 0xEE) {
+            int id = recv_data.mode.id;
-                motor_fbk_.correct = 0;
+            if (id < 0 || id >= MOTOR_NUM) {
+                buffer_offset -= packet_size;
+                std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset);
+                continue;
+            }
+
+            if (recv_data.head[0] != 0xFD || recv_data.head[1] != 0xEE) {
+                motor_fbk_[port_idx][id].correct = 0;
             } else {
-                motor_fbk_.calc_crc = crc_ccitt::crc_ccitt(
+                motor_fbk_[port_idx][id].calc_crc = crc_ccitt::crc_ccitt(
-                    0, (uint8_t *)&motor_fbk_.motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_fbk_.motor_recv_data.CRC16));
+                    0, (uint8_t *)&recv_data, sizeof(RIS_MotorData_t) - sizeof(recv_data.CRC16));
-                if (motor_fbk_.motor_recv_data.CRC16 != motor_fbk_.calc_crc) {
+                if (recv_data.CRC16 != motor_fbk_[port_idx][id].calc_crc) {
-                    memset(&motor_fbk_.motor_recv_data, 0, sizeof(RIS_MotorData_t));
+                    memset(&motor_fbk_[port_idx][id].motor_recv_data, 0, sizeof(RIS_MotorData_t));
-                    motor_fbk_.correct = 0;
+                    motor_fbk_[port_idx][id].correct = 0;
-                    motor_fbk_.bad_msg++;
+                    motor_fbk_[port_idx][id].bad_msg++;
                 } else {
-                    motor_fbk_.motor_id = motor_fbk_.motor_recv_data.mode.id;
+                    std::memcpy(&motor_fbk_[port_idx][id].motor_recv_data, &recv_data, packet_size);
-                    motor_fbk_.mode = motor_fbk_.motor_recv_data.mode.status;
+                    motor_fbk_[port_idx][id].motor_id = recv_data.mode.id;
-                    motor_fbk_.Temp = motor_fbk_.motor_recv_data.fbk.temp;
+                    motor_fbk_[port_idx][id].mode = recv_data.mode.status;
-                    motor_fbk_.MError = motor_fbk_.motor_recv_data.fbk.MError;
+                    motor_fbk_[port_idx][id].Temp = recv_data.fbk.temp;
-                    motor_fbk_.W = ((float)motor_fbk_.motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
+                    motor_fbk_[port_idx][id].MError = recv_data.fbk.MError;
-                    motor_fbk_.T = ((float)motor_fbk_.motor_recv_data.fbk.torque) / 256.0f;
+                    motor_fbk_[port_idx][id].W = ((float)recv_data.fbk.speed / 256.0f) * 6.28318f;
-                    motor_fbk_.Pos = 6.28318f * ((float)motor_fbk_.motor_recv_data.fbk.pos) / 32768.0f;
+                    motor_fbk_[port_idx][id].T = ((float)recv_data.fbk.torque) / 256.0f;
-                    motor_fbk_.footForce = motor_fbk_.motor_recv_data.fbk.force;
+                    motor_fbk_[port_idx][id].Pos = 6.28318f * ((float)recv_data.fbk.pos) / 32768.0f;
-                    motor_fbk_.correct = 1;
+                    motor_fbk_[port_idx][id].footForce = recv_data.fbk.force;
+                    motor_fbk_[port_idx][id].correct = 1;
                 }
             }
-            if (motor_fbk_.correct) {
+            if (motor_fbk_[port_idx][id].correct) {
-                // 发布反馈消息
+                recv_count_[port_idx]++;
-                rc_msgs::msg::GoMotorFdb msg;
+                RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
-                msg.torque = motor_fbk_.T;
+                    "[Port%d] Motor ID: %d, Position: %f", port_idx, motor_fbk_[port_idx][id].motor_id, motor_fbk_[port_idx][id].Pos);
-                msg.speed = motor_fbk_.W;
-                msg.pos = motor_fbk_.Pos;
-                motor_fdb_pub_->publish(msg);
-                recv_count_++;
-                RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Motor ID: %d, Position: %f", motor_fbk_.motor_id, motor_fbk_.Pos);
             }
             std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size);
             buffer_offset -= packet_size;
         } catch (const std::exception &e) {
-            RCLCPP_ERROR(this->get_logger(), "Serial read exception: %s", e.what());
+            RCLCPP_ERROR(this->get_logger(), "Serial read exception [%d]: %s", port_idx, e.what());
-            reopen_port();
+            reopen_port(port_idx);
         }
     }
 }
 
-void UnitreeLegSerial::reopen_port()
+void UnitreeLegSerial::reopen_port(int port_idx)
 {
-    close_serial_port();
+    close_serial_port(port_idx);
     rclcpp::sleep_for(std::chrono::milliseconds(100));
-    open_serial_port();
+    open_serial_port(port_idx);
 }
 
 } // namespace unitree_leg_serial

src/rc_msgs/CMakeLists.txt

@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.8)
 project(rc_msgs)
 
 find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
 rosidl_generate_interfaces(${PROJECT_NAME}
         "msg/DataMCU.msg"
         "msg/DataRef.msg"
@@ -11,7 +13,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
         "msg/DataNav.msg"
         "msg/GoMotorCmd.msg"
         "msg/GoMotorFdb.msg"
+        "msg/LegCmd.msg"
+        "msg/LegFdb.msg"
+    DEPENDENCIES std_msgs
 )
 
 ament_package()
-

src/rc_msgs/msg/LegCmd.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+GoMotorCmd[12] leg_cmd

src/rc_msgs/msg/LegFdb.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+GoMotorFdb[12] leg_fdb

src/rc_msgs/package.xml

@@ -5,7 +5,8 @@
     <description>Describe custom messages</description>
     <maintainer email="1683502971@qq.com">biao</maintainer>
     <license>MIT</license>
-
+    <build_depend>std_msgs</build_depend>
+    <exec_depend>std_msgs</exec_depend>
     <buildtool_depend>rosidl_default_generators</buildtool_depend>
     <exec_depend>rosidl_default_runtime</exec_depend>
     <member_of_group>rosidl_interface_packages</member_of_group>

src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py

@@ -40,10 +40,10 @@ def generate_launch_description():
                 }
             ],
         ),
-        Node(
+        # Node(
-            package='joint_state_publisher_gui',
+        #     package='joint_state_publisher_gui',
-            executable='joint_state_publisher_gui',
+        #     executable='joint_state_publisher_gui',
-            name='joint_state_publisher',
+        #     name='joint_state_publisher',
-            output='screen',
+        #     output='screen',
-        )
+        # )
     ])

src/robot_descriptions/qut/mdog_description/urdf/robot.urdf

@@ -39,7 +39,7 @@
   </material>
   <ros2_control name="MdogSystem" type="system">
     <hardware>
-      <plugin>hardware_mdog/hardware_mdog_real</plugin>
+      <plugin>mdog_hardware/MDogHardwareInterface</plugin>
     </hardware>
     <joint name="FR_hip_joint">
       <command_interface name="position"/>
@@ -246,7 +246,7 @@
   </link>
   <!-- 定义thigh_joint -->
   <joint name="FR_thigh_joint" type="revolute">
-    <origin rpy="0 -1.5708 0" xyz="0 0.05 0"/>
+    <origin rpy="0 -0.7854 0" xyz="0 0.05 0"/>
     <parent link="FR_hip"/>
     <child link="FR_thigh"/>
     <axis xyz="0 1 0"/>
@@ -274,7 +274,7 @@
   </link>
   <!-- 定义calf_joint -->
   <joint name="FR_calf_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.25 0 0"/>
+    <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
     <parent link="FR_thigh"/>
     <child link="FR_calf"/>
     <axis xyz="0 1 0"/>
@@ -330,7 +330,7 @@
   </link>
   <!-- 定义thigh_joint -->
   <joint name="FL_thigh_joint" type="revolute">
-    <origin rpy="0 -1.5708 0" xyz="0 -0.05 0"/>
+    <origin rpy="0 -0.7854 0" xyz="0 -0.05 0"/>
     <parent link="FL_hip"/>
     <child link="FL_thigh"/>
     <axis xyz="0 1 0"/>
@@ -358,7 +358,7 @@
   </link>
   <!-- 定义calf_joint -->
   <joint name="FL_calf_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.25 0 0"/>
+    <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
     <parent link="FL_thigh"/>
     <child link="FL_calf"/>
     <axis xyz="0 1 0"/>
@@ -414,7 +414,7 @@
   </link>
   <!-- 定义thigh_joint -->
   <joint name="RR_thigh_joint" type="revolute">
-    <origin rpy="0 -1.5708 0" xyz="0 0.05 0"/>
+    <origin rpy="0 -0.7854 0" xyz="0 0.05 0"/>
     <parent link="RR_hip"/>
     <child link="RR_thigh"/>
     <axis xyz="0 1 0"/>
@@ -442,7 +442,7 @@
   </link>
   <!-- 定义calf_joint -->
   <joint name="RR_calf_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.25 0 0"/>
+    <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
     <parent link="RR_thigh"/>
     <child link="RR_calf"/>
     <axis xyz="0 1 0"/>
@@ -498,7 +498,7 @@
   </link>
   <!-- 定义thigh_joint -->
   <joint name="RL_thigh_joint" type="revolute">
-    <origin rpy="0 -1.5708 0" xyz="0 -0.05 0"/>
+    <origin rpy="0 -0.7854 0" xyz="0 -0.05 0"/>
     <parent link="RL_hip"/>
     <child link="RL_thigh"/>
     <axis xyz="0 1 0"/>
@@ -526,7 +526,7 @@
   </link>
   <!-- 定义calf_joint -->
   <joint name="RL_calf_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.25 0 0"/>
+    <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
     <parent link="RL_thigh"/>
     <child link="RL_calf"/>
     <axis xyz="0 1 0"/>

src/robot_descriptions/qut/mdog_description/xacro/leg.xacro

@@ -35,7 +35,8 @@
 
     <!-- 定义thigh_joint -->
     <joint name="${name}_thigh_joint" type="revolute">
-      <origin rpy="0 -0.7854 0" xyz="0 ${mirror * 0.05} 0"/>
+      <origin rpy="0 -1.5708 0" xyz="0 ${mirror * 0.05} 0"/>
+      <!-- <origin rpy="0 0 0" xyz="0 ${mirror * 0.05} 0"/> -->
       <parent link="${name}_hip"/>
       <child link="${name}_thigh"/>
       <axis xyz="0 1 0"/>
@@ -65,7 +66,8 @@
 
     <!-- 定义calf_joint -->
     <joint name="${name}_calf_joint" type="revolute">
-      <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
+      <!-- <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/> -->
+      <origin rpy="0 0 0" xyz="-0.25 0 0"/>
       <parent link="${name}_thigh"/>
       <child link="${name}_calf"/>
       <axis xyz="0 1 0"/>

src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro

@@ -4,7 +4,7 @@
 	<ros2_control name="MdogSystem" type="system">
 
 		<hardware>
-			<plugin>mdog_hardware/mdog_hardware_interface</plugin>
+			<plugin>mdog_hardware/MDogHardwareInterface</plugin>
 		</hardware>
 
 		<joint name="FR_hip_joint">