有个会卡住的bug，有点烦

.vscode/settings.json

@@ -1,6 +1,7 @@
 {
     "files.associations": {
-        "chrono": "cpp",
-        "functional": "cpp"
+        "deque": "cpp",
+        "string": "cpp",
+        "vector": "cpp"
     }
 }

README.md

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

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -11,6 +11,7 @@ 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(
@@ -20,15 +21,31 @@ include_directories(
 
 add_executable(mdog_simple_controller
   src/mdog_simple_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_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs)
+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}
 )
 

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/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/user_math.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+// 关节0点位置offset和减速比定义
+constexpr float JOINT_OFFSET[4][3] = {
+    {0.0f, 0.0f, 0.0f}, // 可根据实际填写
+    {0.0f, 0.0f, 0.0f},
+    {0.0f, 0.0f, 0.0f},
+    // {0.2f, 0.393f, 0.5507f}
+    {5.63f, 5.29f, 4.34f}
+};
+
+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 cmd → 输出 cmd
+void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst);
+
+} // namespace mdog_simple_controller

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

@@ -4,11 +4,14 @@
 #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_zero.hpp"
 #include "mdog_simple_controller/FSM/state_balance.hpp"
 #include "mdog_simple_controller/FSM/state_troting.hpp"
 
+
 namespace mdog_simple_controller
 {
 
@@ -48,7 +51,9 @@ struct InputCmd {
 
 struct MdogControllerData {
     LegJointData feedback[4];
+    LegJointData feedback_real[4];
     LegJointCmd command[4];
+    LegJointCmd command_real[4];
 };
 
 
@@ -57,7 +62,7 @@ public:
     MdogSimpleController();
 
     void change_state(FSMState* new_state);
-    
+
     MdogControllerData data_;
     InputCmd input_cmd_;
 private:
@@ -74,6 +79,8 @@ private:
     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_;
 };

src/controllers/mdog_simple_controller/package.xml

@@ -13,6 +13,7 @@
   <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>

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

@@ -1,14 +1,74 @@
 #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 {
 
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 1.0;
+}
+
 void StateBalance::enter(MdogSimpleController* ctrl) {
     // 进入BALANCE状态时的初始化
 }
 
 void StateBalance::run(MdogSimpleController* ctrl) {
-    // BALANCE状态下的控制逻辑
+    // 贝塞尔控制点
+    std::vector<std::array<double, 3>> control_points = {
+        {0.0, 0.0, -0.30},      // 起点
+        {0.01, 0.0, -0.05},     // 抬腿中点
+        {0.02, 0.0, -0.30}      // 落点
+    };
+    // 起点和终点
+    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]
+
+    // 对角腿同步，交错运动，相位差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;
+        std::array<double, 3> link = {0.05, 0.25, 0.25}; // 根据实际参数
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            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 StateBalance::exit(MdogSimpleController* ctrl) {

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_zero.cpp

@@ -11,7 +11,7 @@ void StateZero::enter(MdogSimpleController* ctrl) {
             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;
+            ctrl->data_.command[leg].kd[j] = 0.05;
         }
     }
 }

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/user_math.cpp

@@ -0,0 +1,36 @@
+#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 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];
+        }
+    }
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp

@@ -0,0 +1,142 @@
+#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>(
+        "/euler_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);
+
+    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.5;
+    input_cmd_.status = msg->command;
+}
+
+void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
+    input_cmd_.imu_euler.yaw = msg->z;
+    input_cmd_.imu_euler.rol = msg->x;
+    input_cmd_.imu_euler.pit = msg->y;
+}
+
+void MdogSimpleController::control_loop() {
+    FSMState* target_state = nullptr;
+    switch (input_cmd_.status) {
+        case 0: target_state = &state_zero_; break;
+        case 6: target_state = &state_balance_; break;
+        case 7: target_state = &state_troting_; break;
+        default: target_state = &state_zero_; break;
+    }
+    if (target_state != current_state_) {
+        change_state(target_state);
+    }
+    if (current_state_) current_state_->run(this);
+
+    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);
+
+    if (0){
+        RCLCPP_INFO(this->get_logger(), "MdogControllerData:");
+        for (int leg = 0; leg < 4; ++leg) {
+            RCLCPP_INFO(this->get_logger(), "Leg %d:", leg);
+            for (int j = 0; j < 3; ++j) {
+                RCLCPP_INFO(this->get_logger(), "Joint %d: Torque: %f, Speed: %f, Pos: %f",
+                    j, data_.feedback[leg].torque[j], data_.feedback[leg].speed[j], data_.feedback[leg].pos[j]);
+            }
+        }
+        RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d",
+            input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status);
+        RCLCPP_INFO(this->get_logger(), "AHRS: Yaw: %f, Roll: %f, Pitch: %f",
+            input_cmd_.imu_euler.yaw, input_cmd_.imu_euler.rol, input_cmd_.imu_euler.pit);
+    }
+    RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d",
+        input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status);
+    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);
+
+    //将publish的数据直接赋值给feedback
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            data_.feedback_real[leg].pos[j] = data_.command_real[leg].pos_des[j];
+        }
+    }
+
+    //打印真实数据
+    // for (int leg = 0; leg < 4; ++leg) {
+    //     for (int j = 0; j < 3; ++j) {
+    //         RCLCPP_INFO(this->get_logger(), "Leg %d Joint %d: Torque: %f, Speed: %f, Pos: %f",
+    //             leg, j, data_.feedback_real[leg].torque[j], data_.feedback_real[leg].speed[j], data_.feedback_real[leg].pos[j]);
+    //     }
+    // }
+}
+
+} // 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/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -1,4 +1,5 @@
 #include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
 
 namespace mdog_simple_controller
 {
@@ -14,6 +15,8 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
         "/euler_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);
+
     current_state_ = &state_zero_;
     current_state_->enter(this);
     timer_ = this->create_wall_timer(
@@ -36,6 +39,7 @@ void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr ms
             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){
@@ -55,10 +59,10 @@ void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::Shar
 void MdogSimpleController::control_loop() {
     FSMState* target_state = nullptr;
     switch (input_cmd_.status) {
-        case 0: target_state = &state_zero_; break;
+        case 0: target_state = &state_safe_; break;
         case 6: target_state = &state_balance_; break;
         case 7: target_state = &state_troting_; break;
-        default: target_state = &state_zero_; break;
+        default: target_state = &state_safe_; break;
     }
     if (target_state != current_state_) {
         change_state(target_state);
@@ -95,7 +99,29 @@ void MdogSimpleController::control_loop() {
     }
     RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d",
         input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status);
-
+    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);
+    //打印真实数据
+    // for (int leg = 0; leg < 4; ++leg) {
+    //     for (int j = 0; j < 3; ++j) {
+    //         RCLCPP_INFO(this->get_logger(), "Leg %d Joint %d: Torque: %f, Speed: %f, Pos: %f",
+    //             leg, j, data_.feedback_real[leg].torque[j], data_.feedback_real[leg].speed[j], data_.feedback_real[leg].pos[j]);
+    //     }
+    // }
 }
 
 } // namespace mdog_simple_controller

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

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

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 -0.7854 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"/>