refactory robot motion

2026-01-11 10:41:47 +08:00
parent 9da5f42bcf
commit 29dbef1cd6
50 changed files with 2890 additions and 1724 deletions
--- a/src/robot_control/CMakeLists.txt
+++ b/src/robot_control/CMakeLists.txt
@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.16)  # 提升最低版本以匹配ROS 2推荐版本
 project(robot_control)

+# 设置 C++17 标准（std::filesystem 需要）
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
@@ -15,7 +19,6 @@ find_package(trajectory_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(action_msgs REQUIRED) 
-find_package(rosidl_default_generators REQUIRED)
 find_package(interfaces REQUIRED)

 # 头文件目录
@@ -27,15 +30,16 @@ include_directories(

 # 源文件列表
 set(SOURCES
-  src/trapezoidal_trajectory.cpp
-  src/robot_control_manager.cpp  
-  src/robot_control_node.cpp
-  src/arm_control.cpp
-  src/leg_control.cpp
-  src/robot_model.cpp
-  src/wheel_control.cpp
-  src/waist_control.cpp
-  src/control_base.cpp
+  src/core/robot_control_node.cpp
+  src/core/robot_control_manager.cpp
+  src/core/controller_factory.cpp
+  src/actions/action_manager.cpp
+  src/controllers/control_base.cpp
+  src/controllers/leg_control.cpp
+  src/controllers/arm_control.cpp
+  src/controllers/wheel_control.cpp
+  src/controllers/waist_control.cpp
+  src/utils/trapezoidal_trajectory.cpp
  src/main.cpp
 )

--- a/src/robot_control/REFACTORING_GUIDE.md
+++ b/src/robot_control/REFACTORING_GUIDE.md
@@ -0,0 +1,258 @@
+# 重构指南
+
+## 已完成的工作
+
+1. ✅ 创建了新的目录结构（include/actions, include/core, include/controllers, include/utils）
+2. ✅ 创建了 ActionManager 类（头文件和实现文件）
+3. ✅ 创建了 ControllerFactory 类（头文件和实现文件）
+
+## 待完成的工作
+
+### 步骤 1: 移动文件到新目录结构
+
+由于文件依赖关系复杂，建议按以下顺序移动：
+
+#### 1.1 移动工具类文件（utils）
+这些文件依赖最少：
+```bash
+# 移动到 utils 目录
+mv include/trapezoidal_trajectory.hpp include/utils/
+mv src/trapezoidal_trajectory.cpp src/utils/
+
+mv include/robot_model.hpp include/utils/
+mv src/robot_model.cpp src/utils/
+
+mv include/robot_kinematics.hpp include/utils/
+mv src/robot_kinematics.cpp src/utils/
+
+mv include/urdf_parser.hpp include/utils/
+mv src/urdf_parser.cpp src/utils/
+
+mv include/extended_kalman_filter.hpp include/utils/
+mv src/extended_kalman_filter.cpp src/utils/
+
+mv include/deceleration_planner.hpp include/utils/
+```
+
+#### 1.2 移动核心文件（core）
+```bash
+# 移动核心枚举和参数
+mv include/common_enum.hpp include/core/
+mv include/motion_parameters.hpp include/core/
+
+# 移动管理器
+mv include/robot_control_manager.hpp include/core/
+mv src/robot_control_manager.cpp src/core/
+
+# 移动节点（稍后修改）
+# mv include/robot_control_node.hpp include/core/
+# mv src/robot_control_node.cpp src/core/
+```
+
+#### 1.3 移动控制器文件（controllers）
+```bash
+# 移动控制器基类
+mv include/control_base.hpp include/controllers/
+mv src/control_base.cpp src/controllers/
+
+# 移动具体控制器
+mv include/leg_control.hpp include/controllers/
+mv src/leg_control.cpp src/controllers/
+
+mv include/arm_control.hpp include/controllers/
+mv src/arm_control.cpp src/controllers/
+
+mv include/wheel_control.hpp include/controllers/
+mv src/wheel_control.cpp src/controllers/
+
+mv include/waist_control.hpp include/controllers/
+mv src/waist_control.cpp src/controllers/
+```
+
+### 步骤 2: 更新包含路径
+
+所有文件移动后，需要更新包含路径。使用以下脚本批量替换：
+
+```bash
+# 在项目根目录执行
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"control_base\.hpp"|"controllers/control_base.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"leg_control\.hpp"|"controllers/leg_control.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"arm_control\.hpp"|"controllers/arm_control.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"wheel_control\.hpp"|"controllers/wheel_control.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"waist_control\.hpp"|"controllers/waist_control.hpp"|g' {} +
+
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"motion_parameters\.hpp"|"core/motion_parameters.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"common_enum\.hpp"|"core/common_enum.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"robot_control_manager\.hpp"|"core/robot_control_manager.hpp"|g' {} +
+
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"trapezoidal_trajectory\.hpp"|"utils/trapezoidal_trajectory.hpp"|g' {} +
+find include src -type f \( -name "*.hpp" -o -name "*.cpp" \) -exec sed -i 's|"robot_model\.hpp"|"utils/robot_model.hpp"|g' {} +
+```
+
+### 步骤 3: 修改 RobotControlManager 支持动态控制器加载
+
+在 `include/core/robot_control_manager.hpp` 中：
+
+1. 添加控制器工厂的包含：
+```cpp
+#include "core/controller_factory.hpp"
+```
+
+2. 修改私有成员，使用智能指针和可选控制器：
+```cpp
+private:
+    // 控制器（使用智能指针，支持可选加载）
+    std::unique_ptr<LegControl> leg_controller_;
+    std::unique_ptr<WheelControl> wheel_controller_;
+    std::unique_ptr<WaistControl> waist_controller_;
+    std::unique_ptr<ArmControl> arm_controller_;  // 可选
+
+    // 控制器启用标志
+    bool leg_controller_enabled_;
+    bool wheel_controller_enabled_;
+    bool waist_controller_enabled_;
+    bool arm_controller_enabled_;
+```
+
+3. 修改构造函数，添加控制器配置参数：
+```cpp
+RobotControlManager(const std::vector<std::string>& enabled_controllers = {"leg", "wheel", "waist"});
+```
+
+4. 在 `init()` 函数中使用 ControllerFactory 创建控制器：
+```cpp
+void RobotControlManager::init(const std::vector<std::string>& enabled_controllers)
+{
+    // ... 其他初始化代码 ...
+
+    // 动态加载控制器
+    if (ControllerFactory::is_controller_enabled(ControllerType::LEG, enabled_controllers))
+    {
+        leg_controller_ = std::unique_ptr<LegControl>(
+            static_cast<LegControl*>(ControllerFactory::create_controller(
+                ControllerType::LEG, motionParams_).release()));
+        leg_controller_enabled_ = true;
+    }
+
+    if (ControllerFactory::is_controller_enabled(ControllerType::WHEEL, enabled_controllers))
+    {
+        wheel_controller_ = std::unique_ptr<WheelControl>(
+            static_cast<WheelControl*>(ControllerFactory::create_controller(
+                ControllerType::WHEEL, motionParams_).release()));
+        wheel_controller_enabled_ = true;
+    }
+
+    // ... 类似地处理其他控制器 ...
+}
+```
+
+### 步骤 4: 修改 RobotControlNode 使用 ActionManager
+
+在 `include/core/robot_control_node.hpp` 中：
+
+1. 添加 ActionManager 包含：
+```cpp
+#include "actions/action_manager.hpp"
+```
+
+2. 移除所有 action 相关的成员变量和方法声明
+
+3. 添加 ActionManager 成员：
+```cpp
+private:
+    std::unique_ptr<ActionManager> action_manager_;
+```
+
+4. 在构造函数中初始化 ActionManager：
+```cpp
+RobotControlNode::RobotControlNode() : Node("robot_control_node")
+{
+    // ... 其他初始化代码 ...
+
+    // 初始化 ActionManager
+    auto is_jog_mode_func = [this]() -> bool { return isJogMode_; };
+    action_manager_ = std::make_unique<ActionManager>(
+        this,
+        robotControlManager_,
+        is_jog_mode_func,
+        motorCmdPub_,
+        motorParamClient_);
+    action_manager_->initialize();
+}
+```
+
+5. 在 ControlLoop 中使用 ActionManager 的状态：
+```cpp
+void RobotControlNode::ControlLoop()
+{
+    // ...
+    if (action_manager_->is_move_home_executing())
+    {
+        if(robotControlManager_.GoHome(dt_sec))
+        {
+            robotControlManager_.SetJogWheel(false);
+            action_manager_->set_move_home_executing(false);
+            robotControlManager_.WheelReset();
+            robotControlManager_.ImuReset();
+        }
+    }
+    // ... 类似地处理其他 action ...
+}
+```
+
+### 步骤 5: 更新 CMakeLists.txt
+
+1. 更新源文件路径：
+```cmake
+set(SOURCES
+  src/core/robot_control_node.cpp
+  src/core/robot_control_manager.cpp
+  src/core/controller_factory.cpp
+  src/actions/action_manager.cpp
+  src/controllers/control_base.cpp
+  src/controllers/leg_control.cpp
+  src/controllers/arm_control.cpp
+  src/controllers/wheel_control.cpp
+  src/controllers/waist_control.cpp
+  src/utils/trapezoidal_trajectory.cpp
+  src/main.cpp
+)
+```
+
+2. 更新头文件包含目录（如果需要）：
+```cmake
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIRS}
+)
+```
+
+### 步骤 6: 测试和调试
+
+1. 编译项目：
+```bash
+cd /home/demo/hive_core_workspace/hivecore_robot_os1
+colcon build --packages-select robot_control
+```
+
+2. 修复编译错误（主要是包含路径问题）
+
+3. 运行测试确保功能正常
+
+## 注意事项
+
+1. **文件移动顺序很重要**：先移动依赖少的文件，再移动依赖多的文件
+2. **包含路径更新**：移动文件后必须更新所有包含路径
+3. **控制器动态加载**：确保在使用控制器前检查是否启用
+4. **测试**：每完成一步都要测试编译，避免错误累积
+
+## 建议的重构顺序
+
+1. 移动 utils 文件并更新路径
+2. 移动 controllers 文件并更新路径  
+3. 移动 core 文件（除了 robot_control_node）并更新路径
+4. 修改 RobotControlManager 支持动态加载
+5. 移动和修改 robot_control_node
+6. 更新 CMakeLists.txt
+7. 测试编译和运行
+
--- a/src/robot_control/action/MoveHome.action
+++ b/src/robot_control/action/MoveHome.action
@@ -1,8 +0,0 @@
-# 目标 回零点
---
-# 结果：运动成功，或执行完毕
-bool success
-string message
---
-# 反馈：各关节位置，运动进度
-int64[] joint_values
--- a/src/robot_control/action/MoveLeg.action
+++ b/src/robot_control/action/MoveLeg.action
@@ -1,9 +0,0 @@
-# 目标 腿伸长或缩短运动
-float32 move_up_distance
---
-# 结果：运动成功，或执行完毕
-bool success
-string message
---
-# 反馈：各关节角度
-int64[] joint_values
--- a/src/robot_control/action/MoveWaist.action
+++ b/src/robot_control/action/MoveWaist.action
@@ -1,10 +0,0 @@
-# 目标 腰部运动
-float32 move_pitch_degree
-float32 move_yaw_degree
---
-# 结果：运动成功，或执行完毕
-bool success
-string message
---
-# 反馈：各关节角度
-int64[] joint_values
--- a/src/robot_control/action/MoveWheel.action
+++ b/src/robot_control/action/MoveWheel.action
@@ -1,11 +0,0 @@
-# 目标 底盘运动
-float32 move_distance
-float32 move_angle
---
-# 结果：运动成功，或执行完毕
-bool success
-string message
---
-# 反馈：当前位置，当前角度，运动进度
-float32 current_pos
-float32 current_angle
--- a/src/robot_control/include/actions/action_manager.hpp
+++ b/src/robot_control/include/actions/action_manager.hpp
@@ -0,0 +1,222 @@
+/**
+ * @file action_manager.hpp
+ * @brief Action管理器 - 统一管理所有ROS 2 Action服务器
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类负责：
+ * - 创建和管理所有Action服务器（MoveHome, MoveLeg, MoveWaist, MoveWheel）
+ * - 处理Action的目标请求、取消和执行
+ * - 提供Action执行状态查询接口
+ */
+
+#pragma once
+
+#include <memory>
+#include <thread>
+#include <atomic>
+#include <functional>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "interfaces/action/move_home.hpp"
+#include "interfaces/action/move_leg.hpp"
+#include "interfaces/action/move_waist.hpp"
+#include "interfaces/action/move_wheel.hpp"
+#include "interfaces/msg/motor_cmd.hpp"
+#include "interfaces/srv/motor_param.hpp"
+
+using MoveHome = interfaces::action::MoveHome;
+using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
+
+using MoveLeg = interfaces::action::MoveLeg;
+using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
+
+using MoveWaist = interfaces::action::MoveWaist;
+using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
+
+using MoveWheel = interfaces::action::MoveWheel;
+using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
+
+using MotorCmd = interfaces::msg::MotorCmd;
+using MotorParam = interfaces::srv::MotorParam;
+
+namespace robot_control
+{
+    // 前向声明
+    class RobotControlManager;
+
+    /**
+     * @class ActionManager
+     * @brief Action管理器类
+     * 
+     * 统一管理所有ROS 2 Action服务器，包括：
+     * - MoveHome: 回零动作
+     * - MoveLeg: 腿部运动动作
+     * - MoveWaist: 腰部运动动作
+     * - MoveWheel: 轮子运动动作
+     */
+    class ActionManager
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param node ROS 2节点指针
+         * @param robot_control_manager 机器人控制管理器引用
+         * @param is_jog_mode_func 获取Jog模式状态的函数
+         * @param motor_cmd_pub 电机命令发布器
+         * @param motor_param_client 电机参数客户端
+         */
+        ActionManager(
+            rclcpp::Node* node, 
+            RobotControlManager& robot_control_manager,
+            std::function<bool()> is_jog_mode_func,
+            rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
+            rclcpp::Client<MotorParam>::SharedPtr motor_param_client);
+        
+        /**
+         * @brief 析构函数
+         */
+        ~ActionManager();
+
+        /**
+         * @brief 初始化所有Action服务器
+         */
+        void initialize();
+
+        // ==================== 执行状态查询接口 ====================
+        
+        /**
+         * @brief 检查MoveHome是否正在执行
+         * @return true 正在执行，false 未执行
+         */
+        bool is_move_home_executing() const { return move_home_executing_.load(); }
+        
+        /**
+         * @brief 检查MoveLeg是否正在执行
+         * @return true 正在执行，false 未执行
+         */
+        bool is_move_leg_executing() const { return move_leg_executing_.load(); }
+        
+        /**
+         * @brief 检查MoveWaist是否正在执行
+         * @return true 正在执行，false 未执行
+         */
+        bool is_move_waist_executing() const { return move_waist_executing_.load(); }
+        
+        /**
+         * @brief 检查MoveWheel是否正在执行
+         * @return true 正在执行，false 未执行
+         */
+        bool is_move_wheel_executing() const { return move_wheel_executing_.load(); }
+
+        // ==================== 执行状态设置接口 ====================
+        
+        /**
+         * @brief 设置MoveHome执行状态
+         * @param value true 设置为执行中，false 设置为未执行
+         */
+        void set_move_home_executing(bool value) { move_home_executing_.store(value); }
+        
+        /**
+         * @brief 设置MoveLeg执行状态
+         * @param value true 设置为执行中，false 设置为未执行
+         */
+        void set_move_leg_executing(bool value) { move_leg_executing_.store(value); }
+        
+        /**
+         * @brief 设置MoveWaist执行状态
+         * @param value true 设置为执行中，false 设置为未执行
+         */
+        void set_move_waist_executing(bool value) { move_waist_executing_.store(value); }
+        
+        /**
+         * @brief 设置MoveWheel执行状态
+         * @param value true 设置为执行中，false 设置为未执行
+         */
+        void set_move_wheel_executing(bool value) { move_wheel_executing_.store(value); }
+
+    private:
+        // ==================== MoveHome Action处理函数 ====================
+        
+        /**
+         * @brief 处理MoveHome目标请求
+         * @param uuid 目标UUID
+         * @param goal 目标消息
+         * @return GoalResponse 接受或拒绝目标
+         */
+        rclcpp_action::GoalResponse handle_move_home_goal(
+            const rclcpp_action::GoalUUID& uuid,
+            std::shared_ptr<const MoveHome::Goal> goal);
+        
+        /**
+         * @brief 处理MoveHome取消请求
+         * @param goal_handle 目标句柄
+         * @return CancelResponse 接受或拒绝取消
+         */
+        rclcpp_action::CancelResponse handle_move_home_cancel(
+            const std::shared_ptr<GoalHandleMoveHome> goal_handle);
+        
+        /**
+         * @brief 处理MoveHome目标接受
+         * @param goal_handle 目标句柄
+         */
+        void handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
+        
+        /**
+         * @brief 执行MoveHome动作
+         * @param goal_handle 目标句柄
+         */
+        void move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
+
+        // ==================== MoveLeg Action处理函数 ====================
+        
+        rclcpp_action::GoalResponse handle_move_leg_goal(
+            const rclcpp_action::GoalUUID& uuid,
+            std::shared_ptr<const MoveLeg::Goal> goal);
+        rclcpp_action::CancelResponse handle_move_leg_cancel(
+            const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
+        void handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
+        void move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
+
+        // ==================== MoveWaist Action处理函数 ====================
+        
+        rclcpp_action::GoalResponse handle_move_waist_goal(
+            const rclcpp_action::GoalUUID& uuid,
+            std::shared_ptr<const MoveWaist::Goal> goal);
+        rclcpp_action::CancelResponse handle_move_waist_cancel(
+            const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
+        void handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
+        void move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
+
+        // ==================== MoveWheel Action处理函数 ====================
+        
+        rclcpp_action::GoalResponse handle_move_wheel_goal(
+            const rclcpp_action::GoalUUID& uuid,
+            std::shared_ptr<const MoveWheel::Goal> goal);
+        rclcpp_action::CancelResponse handle_move_wheel_cancel(
+            const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
+        void handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
+        void move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
+
+        // ==================== 成员变量 ====================
+        
+        rclcpp::Node* node_;                              ///< ROS 2节点指针
+        RobotControlManager& robot_control_manager_;      ///< 机器人控制管理器引用
+        std::function<bool()> is_jog_mode_func_;          ///< Jog模式状态查询函数
+        rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;        ///< 电机命令发布器
+        rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;    ///< 电机参数客户端
+
+        // Action服务器
+        rclcpp_action::Server<MoveHome>::SharedPtr move_home_action_server_;   ///< MoveHome动作服务器
+        rclcpp_action::Server<MoveLeg>::SharedPtr move_leg_action_server_;     ///< MoveLeg动作服务器
+        rclcpp_action::Server<MoveWaist>::SharedPtr move_waist_action_server_; ///< MoveWaist动作服务器
+        rclcpp_action::Server<MoveWheel>::SharedPtr move_wheel_action_server_; ///< MoveWheel动作服务器
+
+        // Action执行状态标志（使用atomic确保线程安全）
+        std::atomic<bool> move_home_executing_;   ///< MoveHome执行状态
+        std::atomic<bool> move_leg_executing_;    ///< MoveLeg执行状态
+        std::atomic<bool> move_waist_executing_;  ///< MoveWaist执行状态
+        std::atomic<bool> move_wheel_executing_;  ///< MoveWheel执行状态
+    };
+}
--- a/src/robot_control/include/arm_control.hpp
+++ b/src/robot_control/include/arm_control.hpp
@@ -1,32 +0,0 @@
-#pragma once
-
-#include <Eigen/Dense>
-#include "control_base.hpp"  // 包含父类头文件
-
-namespace robot_control
-{
-    class ArmControl : public ControlBase
-    {
-        public:
-            ArmControl(
-                size_t total_joints, 
-                const std::vector<double>& lengthParameters, 
-                const std::vector<double>& maxSpeed,
-                const std::vector<double>& maxAcc,
-                const std::vector<double>& minLimits, 
-                const std::vector<double>& maxLimits, 
-                const std::vector<double>& home_positions,
-                const std::vector<double>& zero_positions,
-                const std::vector<int>& joint_directions
-            );
-
-            ~ArmControl() override;
-
-            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
-
-            bool MoveUp(std::vector<double>& joint_positions, double dt);
-
-            bool MoveDown(std::vector<double>& joint_positions, double dt);
-    };
-
-}
--- a/src/robot_control/include/common_enum.hpp
+++ b/src/robot_control/include/common_enum.hpp
@@ -1,14 +0,0 @@
-#pragma once
-
-namespace robot_control
-{
-    /**
-     * @brief 运动模块枚举
-     */
-    enum class MovementPart {
-        LEG,
-        WAIST,
-        WHEEL,
-        ALL
-    };
-}
--- a/src/robot_control/include/control_base.hpp
+++ b/src/robot_control/include/control_base.hpp
@@ -1,89 +0,0 @@
-#pragma once
-
-#include <Eigen/Dense>
-#include "rclcpp/rclcpp.hpp"
-#include "trapezoidal_trajectory.hpp"
-#include "motion_parameters.hpp"
-
-#define POSITION_TOLERANCE 1.0
-#define TIME_OUT_COUNT 10000
-
-namespace robot_control
-{
-    class ControlBase
-    {
-        // 重要：将成员变量改为 protected，子类可访问但外部不可见
-        protected:
-            size_t total_joints_;                               // 总关节数
-            std::vector<double> lengthParameters_;              // 机械参数（子类可复用）
-            std::vector<double> maxSpeed_;                      // 各关节最大速度
-            std::vector<double> maxAcc_;                        // 各关节最大加速度
-
-            double stopDurationTime_;
-            double movingDurationTime_;
-
-            TrapezoidalTrajectory* trapezoidalTrajectory_;      // 轨迹规划器（子类直接用）
-
-            std::vector<double> joint_home_positions_;          // 回home点位置
-            std::vector<double> joint_zero_positions_;          // 零点位置
-            std::vector<int> joint_directions_;                 // 关节运动方向
-            std::vector<double> joint_position_;                // 当前关节位置
-            std::vector<double> joint_commands_;                // 当前关节指令
-            std::vector<double> joint_velocity_;                // 当前关节速度
-            std::vector<double> joint_velocity_commands_;       // 当前关节速度指令
-            std::vector<double> joint_acceleration_;            // 当前关节加速度
-            std::vector<double> joint_torque_;                  // 当前关节力矩
-            std::vector<double> minLimits_;                     // 关节位置下限
-            std::vector<double> maxLimits_;                     // 关节位置上限
-            std::vector<double> joint_position_desired_;        // 期望关节位置
-            
-            bool is_moving_;                                    // 是否运动中
-            bool is_stopping_;                                  // 是否停止中
-            bool is_target_set_;                                // 是否已设置目标点
-            bool is_cmd_send_finished_; 
-            bool is_joint_position_initialized_;              // 是否已初始化关节位置
-
-            int time_out_count_;                                     // 超时时间
-        public:
-            // 构造函数（子类需调用此构造函数初始化父类）
-            ControlBase(
-                size_t total_joints, 
-                const std::vector<double>& lengthParameters, 
-                const std::vector<double>& maxSpeed,
-                const std::vector<double>& maxAcc,
-                const std::vector<double>& minLimits, 
-                const std::vector<double>& maxLimits, 
-                const std::vector<double>& home_positions,
-                const std::vector<double>& zero_positions,
-                const std::vector<int>& joint_directions
-            );
-
-            virtual ~ControlBase();
-
-            // 需子类重写的函数：声明为 virtual（纯虚函数/普通虚函数）
-            // 1. 笛卡尔空间目标点运动（机械臂需解算运动学，子类重写）
-            virtual bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) = 0;
-
-            // 2. 关节空间目标运动（通用逻辑，父类可实现，子类可重写）
-            virtual bool MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double duration);
-
-            // 3. 回零点（通用逻辑，父类实现）
-            virtual bool GoHome(std::vector<double>& joint_positions, double dt);
-
-            // 4. 停止运动（通用逻辑，父类实现）
-            virtual bool Stop(std::vector<double>& joint_positions, double dt);
-
-            // 5. 更新关节状态（通用逻辑，父类实现）
-            virtual void UpdateJointStates(const std::vector<double>& joint_positions, const std::vector<double>& joint_velocities, const std::vector<double>& joint_torques);
-
-            // 6. 判断是否运动中（通用逻辑，父类实现）
-            virtual bool IsMoving();
-
-            virtual bool IsReached(const std::vector<double>& target_joint); // 判断是否到达目标点
-
-            virtual void SetHomePositions(const std::vector<double>& home_positions);
-
-            bool checkJointLimits(const std::vector<double>& target_joint);
-    };
-
-}
--- a/src/robot_control/include/controllers/arm_control.hpp
+++ b/src/robot_control/include/controllers/arm_control.hpp
@@ -0,0 +1,52 @@
+/**
+ * @file arm_control.hpp
+ * @brief 手臂控制器 - 控制机器人手臂运动
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#pragma once
+
+#include "controllers/control_base.hpp"
+
+namespace robot_control
+{
+    /**
+     * @class ArmControl
+     * @brief 手臂控制器类
+     * 
+     * 继承自ControlBase，提供手臂特定的控制功能。
+     * 当前为占位实现，可根据需要扩展功能。
+     */
+    class ArmControl : public ControlBase
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param total_joints 关节总数
+         * @param lengthParameters 长度参数
+         * @param maxSpeed 最大速度
+         * @param maxAcc 最大加速度
+         * @param minLimits 最小限位
+         * @param maxLimits 最大限位
+         * @param home_positions 回零位置
+         * @param zero_positions 零位置
+         * @param joint_directions 关节方向
+         */
+        ArmControl(
+            size_t total_joints,
+            const std::vector<double>& lengthParameters,
+            const std::vector<double>& maxSpeed,
+            const std::vector<double>& maxAcc,
+            const std::vector<double>& minLimits,
+            const std::vector<double>& maxLimits,
+            const std::vector<double>& home_positions,
+            const std::vector<double>& zero_positions,
+            const std::vector<int>& joint_directions);
+
+        /**
+         * @brief 析构函数
+         */
+        ~ArmControl();
+    };
+}
--- a/src/robot_control/include/controllers/control_base.hpp
+++ b/src/robot_control/include/controllers/control_base.hpp
@@ -0,0 +1,149 @@
+/**
+ * @file control_base.hpp
+ * @brief 控制器基类 - 所有控制器的基类
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类定义了所有控制器的通用接口和功能，包括：
+ * - 关节位置、速度、加速度管理
+ * - 梯形轨迹规划
+ * - 回零运动
+ * - 停止运动
+ */
+
+#pragma once
+
+#include <vector>
+#include <memory>
+#include "utils/trapezoidal_trajectory.hpp"
+
+namespace robot_control
+{
+    /**
+     * @class ControlBase
+     * @brief 控制器基类
+     * 
+     * 所有控制器（LegControl, WheelControl, WaistControl, ArmControl）的基类。
+     * 提供通用的控制功能和接口。
+     */
+    class ControlBase
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param total_joints 关节总数
+         * @param lengthParameters 长度参数
+         * @param maxSpeed 最大速度
+         * @param maxAcc 最大加速度
+         * @param minLimits 最小限位
+         * @param maxLimits 最大限位
+         * @param home_positions 回零位置
+         * @param zero_positions 零位置
+         * @param joint_directions 关节方向
+         */
+        ControlBase(
+            size_t total_joints,
+            const std::vector<double>& lengthParameters,
+            const std::vector<double>& maxSpeed,
+            const std::vector<double>& maxAcc,
+            const std::vector<double>& minLimits,
+            const std::vector<double>& maxLimits,
+            const std::vector<double>& home_positions,
+            const std::vector<double>& zero_positions,
+            const std::vector<int>& joint_directions);
+
+        /**
+         * @brief 虚析构函数
+         */
+        virtual ~ControlBase();
+
+        /**
+         * @brief 移动到目标关节位置
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param target_joint 目标关节位置
+         * @param dt 时间步长（秒）
+         * @return true 到达目标，false 运动进行中
+         */
+        bool MoveToTargetJoint(
+            std::vector<double>& joint_positions, 
+            const std::vector<double>& target_joint, 
+            double dt);
+
+        /**
+         * @brief 回零运动
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param dt 时间步长（秒）
+         * @return true 回零完成，false 回零进行中
+         */
+        bool GoHome(std::vector<double>& joint_positions, double dt);
+
+        /**
+         * @brief 停止运动
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param dt 时间步长（秒）
+         * @return true 完全停止，false 正在停止
+         */
+        bool Stop(std::vector<double>& joint_positions, double dt);
+
+        /**
+         * @brief 更新关节状态
+         * @param joint_positions 关节位置
+         * @param joint_velocities 关节速度
+         * @param joint_torques 关节力矩
+         */
+        void UpdateJointStates(
+            const std::vector<double>& joint_positions,
+            const std::vector<double>& joint_velocities,
+            const std::vector<double>& joint_torques);
+
+        /**
+         * @brief 检查是否在运动
+         * @return true 正在运动，false 未运动
+         */
+        bool IsMoving();
+
+        /**
+         * @brief 检查是否到达目标位置
+         * @param target_joint 目标关节位置
+         * @return true 已到达，false 未到达
+         */
+        bool IsReached(const std::vector<double>& target_joint);
+
+        /**
+         * @brief 检查关节限位
+         * @param joint_positions 关节位置（会被修改以满足限位）
+         */
+        bool checkJointLimits(std::vector<double>& joint_positions);
+
+        /**
+         * @brief 设置回零位置
+         * @param home_positions 回零位置
+         */
+        void SetHomePositions(const std::vector<double>& home_positions);
+
+    protected:
+        size_t total_joints_;  ///< 关节总数
+
+        std::vector<double> joint_position_;        ///< 当前关节位置
+        std::vector<double> joint_velocity_;        ///< 当前关节速度
+        std::vector<double> joint_acceleration_;    ///< 当前关节加速度
+        std::vector<double> joint_torque_;          ///< 当前关节力矩
+        std::vector<double> joint_commands_;        ///< 关节命令
+        std::vector<double> joint_position_desired_; ///< 期望关节位置
+
+        std::vector<double> joint_home_positions_;  ///< 回零位置
+        std::vector<double> joint_min_limits_;      ///< 最小限位
+        std::vector<double> joint_max_limits_;      ///< 最大限位
+
+        TrapezoidalTrajectory* trapezoidalTrajectory_;  ///< 梯形轨迹规划器
+
+        bool is_moving_;          ///< 是否在运动
+        bool is_stopping_;        ///< 是否在停止
+        double movingDurationTime_;  ///< 运动持续时间
+        double stopDurationTime_;    ///< 停止持续时间
+
+        bool is_joint_position_initialized_;  ///< 关节位置是否已初始化
+
+        static constexpr double POSITION_TOLERANCE = 0.01;  ///< 位置容差（弧度）
+    };
+}
--- a/src/robot_control/include/controllers/leg_control.hpp
+++ b/src/robot_control/include/controllers/leg_control.hpp
@@ -0,0 +1,62 @@
+/**
+ * @file leg_control.hpp
+ * @brief 腿部控制器 - 控制机器人腿部运动
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#pragma once
+
+#include "controllers/control_base.hpp"
+
+namespace robot_control
+{
+    /**
+     * @class LegControl
+     * @brief 腿部控制器类
+     * 
+     * 继承自ControlBase，提供腿部特定的控制功能。
+     */
+    class LegControl : public ControlBase
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param total_joints 关节总数
+         * @param lengthParameters 长度参数
+         * @param maxSpeed 最大速度
+         * @param maxAcc 最大加速度
+         * @param minLimits 最小限位
+         * @param maxLimits 最大限位
+         * @param home_positions 回零位置
+         * @param zero_positions 零位置
+         * @param joint_directions 关节方向
+         */
+        LegControl(
+            size_t total_joints,
+            const std::vector<double>& lengthParameters,
+            const std::vector<double>& maxSpeed,
+            const std::vector<double>& maxAcc,
+            const std::vector<double>& minLimits,
+            const std::vector<double>& maxLimits,
+            const std::vector<double>& home_positions,
+            const std::vector<double>& zero_positions,
+            const std::vector<int>& joint_directions);
+
+        /**
+         * @brief 析构函数
+         */
+        ~LegControl();
+
+        /**
+         * @brief 向上移动（相对运动）
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param dt 时间步长（秒）
+         * @return true 移动完成，false 移动进行中
+         */
+        bool MoveUp(std::vector<double>& joint_positions, double dt);
+
+    private:
+        bool SetMoveLegParametersInternal(double moveLegDistance);
+    };
+}
--- a/src/robot_control/include/controllers/waist_control.hpp
+++ b/src/robot_control/include/controllers/waist_control.hpp
@@ -0,0 +1,68 @@
+/**
+ * @file waist_control.hpp
+ * @brief 腰部控制器 - 控制机器人腰部运动
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#pragma once
+
+#include "controllers/control_base.hpp"
+#include <Eigen/Dense>
+
+namespace robot_control
+{
+    /**
+     * @class WaistControl
+     * @brief 腰部控制器类
+     * 
+     * 继承自ControlBase，提供腰部特定的控制功能。
+     */
+    class WaistControl : public ControlBase
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param total_joints 关节总数
+         * @param lengthParameters 长度参数
+         * @param maxSpeed 最大速度
+         * @param maxAcc 最大加速度
+         * @param minLimits 最小限位
+         * @param maxLimits 最大限位
+         * @param home_positions 回零位置
+         * @param zero_positions 零位置
+         * @param joint_directions 关节方向
+         */
+        WaistControl(
+            size_t total_joints,
+            const std::vector<double>& lengthParameters,
+            const std::vector<double>& maxSpeed,
+            const std::vector<double>& maxAcc,
+            const std::vector<double>& minLimits,
+            const std::vector<double>& maxLimits,
+            const std::vector<double>& home_positions,
+            const std::vector<double>& zero_positions,
+            const std::vector<int>& joint_directions);
+
+        /**
+         * @brief 析构函数
+         */
+        ~WaistControl();
+
+        /**
+         * @brief 移动腰部
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param dt 时间步长（秒）
+         * @return true 移动完成，false 移动进行中
+         */
+        bool MoveWaist(std::vector<double>& joint_positions, double dt);
+
+    private:
+        bool SetMoveWaistParametersInternal(double movePitchAngle, double moveYawAngle);
+
+        bool is_target_set_;           ///< 目标是否已设置
+        bool is_cmd_send_finished_;    ///< 命令是否已发送完成
+        int time_out_count_;           ///< 超时计数
+        static constexpr int TIME_OUT_COUNT = 500;  ///< 超时计数阈值
+    };
+}
--- a/src/robot_control/include/controllers/wheel_control.hpp
+++ b/src/robot_control/include/controllers/wheel_control.hpp
@@ -0,0 +1,69 @@
+/**
+ * @file wheel_control.hpp
+ * @brief 轮子控制器 - 控制机器人轮子运动
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#pragma once
+
+#include "controllers/control_base.hpp"
+#include <Eigen/Dense>
+
+namespace robot_control
+{
+    /**
+     * @class WheelControl
+     * @brief 轮子控制器类
+     * 
+     * 继承自ControlBase，提供轮子特定的控制功能。
+     */
+    class WheelControl : public ControlBase
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param total_joints 关节总数
+         * @param lengthParameters 长度参数
+         * @param maxSpeed 最大速度
+         * @param maxAcc 最大加速度
+         * @param minLimits 最小限位
+         * @param maxLimits 最大限位
+         * @param home_positions 回零位置
+         * @param zero_positions 零位置
+         * @param joint_directions 关节方向
+         */
+        WheelControl(
+            size_t total_joints,
+            const std::vector<double>& lengthParameters,
+            const std::vector<double>& maxSpeed,
+            const std::vector<double>& maxAcc,
+            const std::vector<double>& minLimits,
+            const std::vector<double>& maxLimits,
+            const std::vector<double>& home_positions,
+            const std::vector<double>& zero_positions,
+            const std::vector<int>& joint_directions);
+
+        /**
+         * @brief 析构函数
+         */
+        ~WheelControl();
+
+        /**
+         * @brief 移动轮子
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @return true 移动完成，false 移动进行中
+         */
+        bool MoveWheel(std::vector<double>& joint_positions);
+
+        /**
+         * @brief 获取轮子速度比例
+         * @return 速度比例
+         */
+        double GetWheelRatioInternal();
+
+    private:
+        bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
+        bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle);
+    };
+}
--- a/src/robot_control/include/core/common_enum.hpp
+++ b/src/robot_control/include/core/common_enum.hpp
@@ -0,0 +1,53 @@
+/**
+ * @file common_enum.hpp
+ * @brief 通用枚举定义
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该文件包含所有机器人控制相关的通用枚举定义。
+ */
+
+#pragma once
+
+namespace robot_control
+{
+    /**
+     * @enum MovementPart
+     * @brief 运动部位枚举
+     * 
+     * 用于标识机器人不同的运动部位，用于查询和控制特定部位的运动状态。
+     */
+    enum class MovementPart {
+        LEG,    ///< 腿部
+        WHEEL,  ///< 轮子
+        WAIST,  ///< 腰部
+        ALL     ///< 全部部位（用于查询所有部位的运动状态）
+    };
+
+    /**
+     * @enum ControllerType
+     * @brief 控制器类型枚举
+     * 
+     * 用于标识不同类型的控制器，用于创建和管理控制器实例。
+     * 注意：枚举值使用 _CONTROLLER 后缀以区别于 MovementPart 枚举。
+     */
+    enum class ControllerType {
+        LEG_CONTROLLER,      ///< 腿部控制器
+        WHEEL_CONTROLLER,    ///< 轮子控制器
+        WAIST_CONTROLLER,    ///< 腰部控制器
+        ARM_CONTROLLER,      ///< 手臂控制器
+        UNKNOWN_CONTROLLER   ///< 未知类型
+    };
+
+    /**
+     * @enum LimitType
+     * @brief 限制类型枚举
+     * 
+     * 用于标识不同类型的关节限制（位置、速度、力矩）。
+     */
+    enum class LimitType {
+        POSITION,   ///< 位置限制（单位：度/弧度）
+        VELOCITY,   ///< 速度限制（单位：度/秒/弧度/秒）
+        EFFORT      ///< 力矩限制（单位：N·m）
+    };
+}
--- a/src/robot_control/include/core/controller_factory.hpp
+++ b/src/robot_control/include/core/controller_factory.hpp
@@ -0,0 +1,72 @@
+/**
+ * @file controller_factory.hpp
+ * @brief 控制器工厂 - 用于创建和管理控制器实例
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类提供工厂模式来创建不同类型的控制器实例，
+ * 支持动态控制器加载和管理。
+ */
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <map>
+
+#include "core/motion_parameters.hpp"
+#include "core/common_enum.hpp"  // 包含 ControllerType 枚举定义
+#include "controllers/control_base.hpp"
+#include "controllers/leg_control.hpp"
+#include "controllers/wheel_control.hpp"
+#include "controllers/waist_control.hpp"
+#include "controllers/arm_control.hpp"
+
+namespace robot_control
+{
+    /**
+     * @class ControllerFactory
+     * @brief 控制器工厂类
+     * 
+     * 提供静态方法来创建和管理控制器实例。
+     * 使用工厂模式实现控制器的统一创建和管理。
+     */
+    class ControllerFactory
+    {
+    public:
+        /**
+         * @brief 创建指定类型的控制器
+         * @param type 控制器类型
+         * @param motion_params 运动参数
+         * @return 控制器智能指针，如果类型无效返回nullptr
+         */
+        static std::unique_ptr<ControlBase> create_controller(
+            ControllerType type, 
+            const MotionParameters& motion_params);
+        
+        /**
+         * @brief 检查指定控制器是否在启用列表中
+         * @param type 控制器类型
+         * @param enabled_controllers 启用的控制器名称列表
+         * @return true 已启用，false 未启用
+         */
+        static bool is_controller_enabled(
+            ControllerType type, 
+            const std::vector<std::string>& enabled_controllers);
+        
+        /**
+         * @brief 将控制器类型转换为字符串
+         * @param type 控制器类型
+         * @return 控制器类型字符串（小写）
+         */
+        static std::string controller_type_to_string(ControllerType type);
+        
+        /**
+         * @brief 将字符串转换为控制器类型
+         * @param type_str 控制器类型字符串
+         * @return 控制器类型，如果无效返回UNKNOWN
+         */
+        static ControllerType string_to_controller_type(const std::string& type_str);
+    };
+}
--- a/src/robot_control/include/core/motion_parameters.hpp
+++ b/src/robot_control/include/core/motion_parameters.hpp
@@ -0,0 +1,329 @@
+/**
+ * @file motion_parameters.hpp
+ * @brief 运动参数类 - 定义机器人运动控制的所有参数
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类包含机器人运动控制所需的所有参数，包括：
+ * - 关节索引和方向
+ * - 关节限位、速度和加速度
+ * - 初始位置和零点位置
+ * - 轮子控制参数
+ * - 关节传动比和分辨率
+ */
+
+#pragma once
+
+#include <vector>
+#include <iostream>
+#include <map>
+#include <string>
+#include "core/common_enum.hpp"  // 包含 LimitType 枚举定义
+
+// ==================== 宏定义 ====================
+
+#define CYCLE_TIME 8    ///< 插补周期（毫秒）
+
+#ifndef DEG2RAD
+#define DEG2RAD(x) ((x)*0.017453293)  ///< 度转弧度
+#endif
+
+#ifndef RAD2DEG
+#define RAD2DEG(x) ((x)*57.29578)     ///< 弧度转度
+#endif
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846   ///< 圆周率
+#endif
+
+namespace robot_control 
+{
+    // ==================== 结构体定义 ====================
+
+    /**
+     * @struct JointLimit
+     * @brief 关节限制参数结构体
+     * 
+     * 包含关节的限制参数：最大值、最小值和限制类型。
+     */
+    struct JointLimit {
+        int index;              ///< 关节索引
+        double max;             ///< 关节运动范围上限（如位置最大角度）
+        double min;             ///< 关节运动范围下限（如位置最小角度）
+        LimitType limit_type;   ///< 限制类型（区分位置/速度/力矩）
+
+        /**
+         * @brief 默认构造函数
+         * 初始化所有成员为默认值（避免未定义值）
+         */
+        JointLimit() : index(0), max(0.0), min(0.0), limit_type(LimitType::POSITION) {}
+
+        /**
+         * @brief 带参构造函数
+         * @param i 关节索引
+         * @param max_val 最大值
+         * @param min_val 最小值
+         * @param type 限制类型
+         */
+        JointLimit(int i, double max_val, double min_val, LimitType type) 
+            : index(i), max(max_val), min(min_val), limit_type(type) 
+        {
+            // 如果最大值小于最小值，交换它们并输出警告
+            if (max < min) 
+            {
+                std::cerr << "[Warning] JointLimit: max (" << max << ") < min (" << min << "), swapping values!" << std::endl;
+                std::swap(max, min);
+            }
+        }
+    };
+
+    // ==================== 类定义 ====================
+
+    /**
+     * @class MotionParameters
+     * @brief 运动参数类
+     * 
+     * 包含机器人运动控制所需的所有参数配置。
+     */
+    class MotionParameters 
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * 
+         * 初始化所有运动参数，包括：
+         * - 关节索引和方向
+         * - 关节限位、速度和加速度
+         * - 初始位置和零点位置
+         * - 轮子控制参数
+         * - 关节传动比和分辨率
+         */
+        MotionParameters()
+        {
+            // ==================== 结构参数 ====================
+            wheel_radius_ = 0.09;        // 轮子半径（米）
+            wheel_separation_ = 0.55;    // 轮距（米）
+
+            wheel_length_ = {
+                wheel_radius_,
+                wheel_separation_
+            };
+
+            // ==================== 尺寸参数 ====================
+            // 腿长参数（单位：米）
+            leg_length_ = {
+                0.66,   // 后腿长
+                0.385,  // 大腿长
+                0.362,  // 小腿长
+                0.55,   // 前腿髋部长度
+                0.4     // 前腿大腿长度
+            };
+
+            waist_length_ = {
+                0.1,    // 腰部长度
+            };
+
+            // ==================== 速度参数 ====================
+            // 轮子速度参数
+            max_linear_speed_x_ = 100;    // 最大线速度 X（m/s）
+            max_linear_speed_z_ = 10;     // 最大线速度 Z（m/s）
+            max_angular_speed_z_ = 50;    // 最大角速度 Z（rad/s）
+
+            // 关节速度参数（度/秒）
+            max_joint_velocity_ = {
+                30, 30, 50, 50, 50, 50, 50, 50
+            }; 
+
+            // 关节加速度参数（度/秒²）
+            max_joint_acceleration_ = {
+                80, 80, 100, 100, 100, 100, 100, 100
+            };
+
+            // ==================== 关节索引 ====================
+            wheel_joint_indices_ = {0, 1}; 
+            waist_joint_indices_ = {0, 1}; 
+            leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
+            total_joints_ = 8; 
+
+            // ==================== 关节方向 ====================
+            wheel_joint_directions_ = {1, -1};  
+            waist_joint_directions_ = {1, 1};
+            leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};  
+
+            // ==================== 关节传动参数 ====================
+            joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比
+            joint_resolution_ = {524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288};  // 分辨率
+
+            // ==================== 关节限位 ====================
+            joint_limits_ = {
+                JointLimit(0, 40.0,    -40.0,   LimitType::POSITION),
+                JointLimit(1, 190.0,   -190.0,  LimitType::POSITION),
+                JointLimit(2, 0.0,     -90.0,   LimitType::POSITION),
+                JointLimit(3, 90.0,    0.0,     LimitType::POSITION),
+                JointLimit(4, 60,      0.0,     LimitType::POSITION),
+                JointLimit(5, 0.0,     -90.0,   LimitType::POSITION),
+                JointLimit(6, 0.0,     -60.0,   LimitType::POSITION),
+                JointLimit(7, 90.0,    0.0,     LimitType::POSITION),
+            };
+
+            // ==================== 初始化限制参数 ====================
+            wheel_max_velocity_.resize(wheel_joint_indices_.size());
+            wheel_max_acceleration_.resize(wheel_joint_indices_.size());
+            wheel_max_limit_.resize(wheel_joint_indices_.size());
+            wheel_min_limit_.resize(wheel_joint_indices_.size());
+
+            wheel_max_velocity_ = {5, 5};
+            wheel_max_acceleration_ = {25, 25};
+            wheel_max_limit_ = {0.0, 0.0};  // 轮子无限位
+            wheel_min_limit_ = {0.0, 0.0};  // 轮子无限位
+
+            waist_min_limit_.resize(waist_joint_indices_.size());
+            waist_max_limit_.resize(waist_joint_indices_.size());
+            waist_max_velocity_.resize(waist_joint_indices_.size());
+            waist_max_acceleration_.resize(waist_joint_indices_.size());
+
+            leg_max_limit_.resize(leg_joint_indices_.size());
+            leg_min_limit_.resize(leg_joint_indices_.size());
+            leg_max_velocity_.resize(leg_joint_indices_.size());
+            leg_max_acceleration_.resize(leg_joint_indices_.size());
+            
+            // 从 joint_limits_ 中提取各部分的限位和速度参数
+            for (size_t i = 0; i < waist_joint_indices_.size(); i++)
+            {
+                waist_max_limit_[i] = joint_limits_[waist_joint_indices_[i]].max;
+                waist_min_limit_[i] = joint_limits_[waist_joint_indices_[i]].min;
+                waist_max_velocity_[i] = max_joint_velocity_[waist_joint_indices_[i]];
+                waist_max_acceleration_[i] = max_joint_acceleration_[waist_joint_indices_[i]];
+            }
+
+            for (size_t i = 0; i < leg_joint_indices_.size(); i++)
+            {
+                leg_max_limit_[i] = joint_limits_[leg_joint_indices_[i]].max;
+                leg_min_limit_[i] = joint_limits_[leg_joint_indices_[i]].min;
+                leg_max_velocity_[i] = max_joint_velocity_[leg_joint_indices_[i]];
+                leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
+            }
+            
+            // ==================== 零点位置 ====================
+            waist_zero_positions_ = {
+                350.0,
+                261.82
+            };
+
+            leg_zero_positions_ = {
+                217.52,
+                120.84,
+                108.7,
+                221.95,
+                234.14,
+                125.39
+            };
+
+            wheel_zero_positions_ = {
+                0.0,
+                0.0
+            };
+
+            // ==================== 初始位置（Home位置） ====================
+            waist_home_positions_ = {
+                350.0,
+                261.82
+            };
+
+            // 后腿零点为站直时候的状态，然后 + 25度，后腿长 0.66m
+            // 前腿零点为站直时候的状态，然后髋部 pitch + 49度，大腿长 0.38m
+            // 膝盖 pitch 再收回 35.5 度，剩余 13.5度长度为 0.35m
+            leg_home_positions_ = {
+                217.52 - 65.0, 
+                120.84 + 41.0, 
+                108.7 + 40.63,
+                221.95 - 41.0, 
+                234.14 - 29.504,
+                125.39 + 65.0
+            };
+
+            wheel_home_positions_ = {
+                0.0,
+                0.0
+            };
+
+            // ==================== 转换因子 ====================
+            pulse_to_degree_.resize(joint_resolution_.size());
+            degree_to_pulse_.resize(joint_resolution_.size());
+
+            for (size_t i = 0; i < joint_resolution_.size(); i++)
+            {
+                degree_to_pulse_[i] = joint_resolution_[i] / 360.0;
+                pulse_to_degree_[i] = 360.0 / joint_resolution_[i];
+            }
+
+            // ==================== Jog参数 ====================
+            jog_step_size_ = 10.0 / (1000.0 / CYCLE_TIME);  // 5度每秒
+        }
+
+        // ==================== 公共成员变量 ====================
+
+        // ---------- 关节索引和方向 ----------
+        size_t total_joints_;                          ///< 总关节数
+        std::vector<int> leg_joint_indices_;           ///< 腿部关节索引
+        std::vector<int> wheel_joint_indices_;         ///< 轮子关节索引
+        std::vector<int> waist_joint_indices_;         ///< 腰部关节索引
+        std::vector<int> real_waist_joint_indices_;    ///< 实际腰部关节索引
+        std::vector<int> real_leg_joint_indices_;      ///< 实际腿部关节索引
+
+        std::vector<int> wheel_joint_directions_;      ///< 轮子关节方向
+        std::vector<int> waist_joint_directions_;      ///< 腰部关节方向
+        std::vector<int> leg_joint_directions_;        ///< 腿部关节方向
+
+        // ---------- 位置参数 ----------
+        std::vector<double> leg_home_positions_;       ///< 腿部初始位置（Home位置）
+        std::vector<double> waist_home_positions_;     ///< 腰部初始位置（Home位置）
+        std::vector<double> wheel_home_positions_;     ///< 轮子初始位置（Home位置）
+
+        std::vector<double> waist_zero_positions_;     ///< 腰部零点位置
+        std::vector<double> leg_zero_positions_;       ///< 腿部零点位置
+        std::vector<double> wheel_zero_positions_;     ///< 轮子零点位置
+
+        // ---------- 限位参数 ----------
+        std::vector<JointLimit> joint_limits_;         ///< 关节限制（包含所有关节）
+        std::vector<double> waist_min_limit_;          ///< 腰部最小限位
+        std::vector<double> waist_max_limit_;          ///< 腰部最大限位
+        std::vector<double> leg_min_limit_;            ///< 腿部最小限位
+        std::vector<double> leg_max_limit_;            ///< 腿部最大限位
+        std::vector<double> wheel_min_limit_;          ///< 轮子最小限位
+        std::vector<double> wheel_max_limit_;          ///< 轮子最大限位
+
+        // ---------- 速度参数 ----------
+        std::vector<double> max_joint_velocity_;       ///< 所有关节的最大速度
+        std::vector<double> waist_max_velocity_;       ///< 腰部最大速度
+        std::vector<double> leg_max_velocity_;         ///< 腿部最大速度
+        std::vector<double> wheel_max_velocity_;       ///< 轮子最大速度
+
+        // ---------- 加速度参数 ----------
+        std::vector<double> max_joint_acceleration_;   ///< 所有关节的最大加速度
+        std::vector<double> waist_max_acceleration_;   ///< 腰部最大加速度
+        std::vector<double> leg_max_acceleration_;     ///< 腿部最大加速度
+        std::vector<double> wheel_max_acceleration_;   ///< 轮子最大加速度
+
+        // ---------- 轮子控制参数 ----------
+        double wheel_radius_;              ///< 轮子半径（米）
+        double wheel_separation_;          ///< 轮距（米）
+        double max_linear_speed_x_;        ///< 最大线速度 X（m/s）
+        double max_linear_speed_z_;        ///< 最大线速度 Z（m/s）
+        double max_angular_speed_z_;       ///< 最大角速度 Z（rad/s）
+
+        // ---------- 尺寸参数 ----------
+        std::vector<double> leg_length_;   ///< 腿部长度参数（米）
+        std::vector<double> waist_length_; ///< 腰部长度参数（米）
+        std::vector<double> wheel_length_; ///< 轮子长度参数（米）
+
+        // ---------- 关节传动参数 ----------
+        std::vector<double> joint_gear_ratio_;     ///< 关节传动比
+        std::vector<double> joint_resolution_;     ///< 关节分辨率（编码器分辨率）
+        std::vector<double> degree_to_pulse_;      ///< 角度转脉冲的转换因子
+        std::vector<double> pulse_to_degree_;      ///< 脉冲转角度的转换因子
+
+        // ---------- Jog参数 ----------
+        double jog_step_size_;             ///< Jog步长（度）
+    };
+}
--- a/src/robot_control/include/core/robot_control_manager.hpp
+++ b/src/robot_control/include/core/robot_control_manager.hpp
@@ -0,0 +1,305 @@
+/**
+ * @file robot_control_manager.hpp
+ * @brief 机器人控制管理器 - 统一管理所有控制器（腿、轮、腰、臂）
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类负责：
+ * - 初始化和管理各个控制器（Leg, Wheel, Waist, Arm）
+ * - 协调不同控制器之间的运动
+ * - 提供统一的接口进行机器人控制
+ * - 处理关节状态更新和命令发布
+ */
+
+#pragma once
+
+#include <cmath>
+#include <memory>
+#include <vector>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "core/motion_parameters.hpp"
+#include "controllers/arm_control.hpp"
+#include "controllers/leg_control.hpp"
+#include "controllers/wheel_control.hpp"
+#include "controllers/waist_control.hpp"
+#include "utils/robot_model.hpp"
+#include "core/common_enum.hpp"
+#include "core/controller_factory.hpp"
+
+#include "interfaces/msg/motor_pos.hpp"
+#include "interfaces/msg/imu_msg.hpp"
+
+using MotorPos = interfaces::msg::MotorPos;
+using ImuMsg = sensor_msgs::msg::Imu;
+
+namespace robot_control
+{
+    /**
+     * @class RobotControlManager
+     * @brief 机器人控制管理器类
+     * 
+     * 统一管理机器人的所有控制器，协调各控制器的运动，
+     * 处理关节状态更新，提供统一的控制接口。
+     */
+    class RobotControlManager
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param enabled_controllers 启用的控制器列表，默认为 {"leg", "wheel", "waist"}
+         */
+        explicit RobotControlManager(const std::vector<std::string>& enabled_controllers = {"leg", "wheel", "waist"});
+        
+        /**
+         * @brief 析构函数
+         */
+        ~RobotControlManager();
+
+        // ==================== 运动控制接口 ====================
+        
+        /**
+         * @brief 控制腿部运动
+         * @param dt 时间步长（秒）
+         * @return true 运动完成，false 运动进行中
+         */
+        bool MoveLeg(double dt);
+        
+        /**
+         * @brief 控制腰部运动
+         * @param dt 时间步长（秒）
+         * @return true 运动完成，false 运动进行中
+         */
+        bool MoveWaist(double dt);
+        
+        /**
+         * @brief 控制轮子运动
+         * @return true 运动完成，false 运动进行中
+         */
+        bool MoveWheel();
+        
+        /**
+         * @brief 停止所有运动
+         * @param dt 时间步长（秒）
+         * @return true 完全停止，false 正在停止
+         */
+        bool Stop(double dt);
+        
+        /**
+         * @brief 回零运动
+         * @param dt 时间步长（秒）
+         * @return true 回零完成，false 回零进行中
+         */
+        bool GoHome(double dt);
+        
+        /**
+         * @brief 点动控制指定轴
+         * @param axis 轴索引
+         * @param dir 方向（-1, 0, 1）
+         */
+        void JogAxis(size_t axis, int dir);
+        
+        /**
+         * @brief 设置轮子点动模式
+         * @param value true 启用点动，false 禁用点动
+         */
+        void SetJogWheel(bool value);
+        
+        /**
+         * @brief 获取轮子点动模式状态
+         * @return true 点动模式启用，false 点动模式禁用
+         */
+        bool GetJogWheel();
+
+        // ==================== 复位接口 ====================
+        
+        /**
+         * @brief 重置轮子回零标志
+         */
+        void WheelReset() { isWheelHomed_ = false; }
+        
+        /**
+         * @brief 重置IMU初始化标志
+         */
+        void ImuReset() { isGyroInited_ = false; }
+
+        // ==================== 参数设置接口 ====================
+        
+        /**
+         * @brief 设置轮子运动参数
+         * @param moveWheelDistance 移动距离
+         * @param moveWheelAngle 移动角度
+         * @return true 参数设置成功，false 参数设置失败
+         */
+        bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
+        
+        /**
+         * @brief 设置腿部运动参数
+         * @param moveLegDistance 移动距离
+         * @return true 参数设置成功，false 参数设置失败
+         */
+        bool SetMoveLegParameters(double moveLegDistance);
+        
+        /**
+         * @brief 设置腰部运动参数
+         * @param movePitchAngle 俯仰角
+         * @param moveYawAngle 偏航角
+         * @return true 参数设置成功，false 参数设置失败
+         */
+        bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
+        
+        // ==================== 状态查询接口 ====================
+        
+        /**
+         * @brief 获取关节命令
+         * @return 关节命令数组
+         */
+        std_msgs::msg::Float64MultiArray GetJointCommands();
+        
+        /**
+         * @brief 获取关节反馈
+         * @return 关节反馈数组
+         */
+        std_msgs::msg::Float64MultiArray GetJointFeedback();
+        
+        /**
+         * @brief 获取轮子命令
+         * @return 轮子命令数组
+         */
+        std_msgs::msg::Float64MultiArray GetWheelCommands();
+        
+        /**
+         * @brief 获取轮子反馈
+         * @return 轮子反馈数组
+         */
+        std_msgs::msg::Float64MultiArray GetWheelFeedback();
+        
+        /**
+         * @brief 获取轮子速度比例
+         * @return 速度比例
+         */
+        double GetWheelRatio();
+
+        /**
+         * @brief 检查关节限位
+         * @param jointCommands 关节命令（会被修改）
+         */
+        void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
+        
+        /**
+         * @brief 更新关节状态（从JointState消息）
+         * @param msg 关节状态消息
+         */
+        void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
+        
+        /**
+         * @brief 更新轮子状态（从MotorPos消息）
+         * @param msg 电机位置消息
+         */
+        void UpdateWheelStates(const MotorPos::SharedPtr msg);
+        
+        /**
+         * @brief 更新IMU消息
+         * @param msg IMU消息
+         */
+        void UpdateImuMsg(const ImuMsg::SharedPtr msg);
+
+        /**
+         * @brief 获取运动参数
+         * @return 运动参数结构
+         */
+        MotionParameters GetMotionParameters();
+
+        /**
+         * @brief 获取IMU差值
+         * @return IMU差值向量
+         */
+        std::vector<double> GetImuDifference();
+
+        /**
+         * @brief 检查指定部位是否在运动
+         * @param part 运动部位（LEG, WHEEL, WAIST, ALL）
+         * @return true 正在运动，false 未运动
+         */
+        bool IsMoving(MovementPart part);
+
+        /**
+         * @brief 检查机器人初始化是否完成
+         * @return true 初始化完成，false 初始化未完成
+         */
+        bool RobotInitFinished();
+
+    private:
+        /**
+         * @brief 初始化函数
+         * @param enabled_controllers 启用的控制器列表
+         */
+        void init(const std::vector<std::string>& enabled_controllers);
+
+        /**
+         * @brief 分配临时值（从关节命令中提取各控制器对应的值）
+         */
+        void AssignTempValues();
+
+        /**
+         * @brief 更新关节命令（将各控制器的临时值合并到关节命令中）
+         */
+        void UpdateJointCommands();
+
+        // ==================== 成员变量 ====================
+        
+        MotionParameters motionParams_;  ///< 运动参数配置
+
+        bool isWaistHomed_;  ///< 腰部是否已回零
+        bool isLegHomed_;    ///< 腿部是否已回零
+        bool isWheelHomed_;  ///< 轮子是否已回零
+
+        bool is_wheel_jog_;  ///< 轮子点动模式标志
+
+        // 控制器（使用智能指针，支持可选加载）
+        std::unique_ptr<LegControl> leg_controller_;     ///< 腿部控制器
+        std::unique_ptr<WheelControl> wheel_controller_; ///< 轮子控制器
+        std::unique_ptr<WaistControl> waist_controller_; ///< 腰部控制器
+        std::unique_ptr<ArmControl> arm_controller_;     ///< 手臂控制器（可选）
+
+        // 控制器启用标志
+        bool leg_controller_enabled_;     ///< 腿部控制器是否启用
+        bool wheel_controller_enabled_;   ///< 轮子控制器是否启用
+        bool waist_controller_enabled_;   ///< 腰部控制器是否启用
+        bool arm_controller_enabled_;     ///< 手臂控制器是否启用
+        
+        // 运动状态
+        std::vector<double> wheelPositions_;   ///< 轮子位置
+        std::vector<double> jointPositions_;   ///< 关节位置（弧度）
+        std::vector<double> jointVelocities_;  ///< 关节速度（弧度/秒）
+        std::vector<double> wheelVelocities_;  ///< 轮子速度（弧度/秒）
+        std::vector<double> jointAccelerations_;  ///< 关节加速度（弧度/秒^2）
+        std::vector<double> wheelAccelerations_;  ///< 轮子加速度（弧度/秒^2）
+        std::vector<double> jointEfforts_;     ///< 关节力矩
+        std::vector<double> wheelEfforts_;     ///< 轮子力矩
+
+        // 关节命令和状态
+        std_msgs::msg::Float64MultiArray jointCommands_;  ///< 关节命令
+        std_msgs::msg::Float64MultiArray wheelCommands_;  ///< 轮子命令
+        sensor_msgs::msg::JointState jointStates_;        ///< 关节状态
+
+        // 临时变量（用于在各控制器之间传递数据）
+        std::vector<double> tempWaistCmd_;  ///< 腰部临时命令
+        std::vector<double> tempLegCmd_;    ///< 腿部临时命令
+        std::vector<double> tempWheelCmd_;  ///< 轮子临时命令
+
+        // IMU相关
+        std::vector<double> gyroValues_;        ///< 陀螺仪值
+        std::vector<double> gyroVelocities_;    ///< 陀螺仪速度
+        std::vector<double> gyroAccelerations_; ///< 陀螺仪加速度
+        bool isGyroInited_;                     ///< 陀螺仪是否已初始化
+
+        // 初始化相关
+        std::vector<bool> jointInited_;  ///< 各关节是否已初始化
+        bool isJointInitValueSet_;       ///< 关节初始化值是否已设置
+    };
+}
--- a/src/robot_control/include/core/robot_control_node.hpp
+++ b/src/robot_control/include/core/robot_control_node.hpp
@@ -0,0 +1,153 @@
+/**
+ * @file robot_control_node.hpp
+ * @brief ROS 2 机器人控制节点 - 主控制节点
+ * @author Robot Control Team
+ * @date 2024
+ * 
+ * 该类是ROS 2节点，负责：
+ * - 创建和管理ROS 2话题、服务和动作服务器
+ * - 协调ActionManager和RobotControlManager
+ * - 处理传感器数据回调
+ * - 执行控制循环
+ */
+
+#pragma once
+
+#include <filesystem>
+#include <fstream>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "core/robot_control_manager.hpp"
+
+#include "interfaces/action/move_home.hpp"
+#include "interfaces/action/move_leg.hpp"
+#include "interfaces/action/move_waist.hpp"
+#include "interfaces/action/move_wheel.hpp"
+#include "interfaces/msg/motor_cmd.hpp"
+#include "interfaces/srv/motor_param.hpp"
+
+#define RECORD_FLAG 0  ///< 数据记录标志（0=禁用，1=启用）
+
+using MoveHome = interfaces::action::MoveHome;
+using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
+
+using MoveLeg = interfaces::action::MoveLeg;
+using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
+
+using MoveWaist = interfaces::action::MoveWaist;
+using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
+
+using MoveWheel = interfaces::action::MoveWheel;
+using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
+
+using MotorCmd = interfaces::msg::MotorCmd;
+using MotorParam = interfaces::srv::MotorParam;
+
+namespace robot_control
+{
+    // 前向声明
+    class ActionManager;
+
+    /**
+     * @class RobotControlNode
+     * @brief ROS 2机器人控制节点类
+     * 
+     * 这是主要的ROS 2节点，负责：
+     * - 管理ROS 2通信（发布器、订阅器、客户端、动作服务器）
+     * - 执行控制循环
+     * - 处理传感器数据
+     * - 协调ActionManager和RobotControlManager
+     */
+    class RobotControlNode : public rclcpp::Node 
+    {
+    public:
+        /**
+         * @brief 构造函数
+         */
+        RobotControlNode();
+        
+        /**
+         * @brief 析构函数
+         */
+        ~RobotControlNode();
+        
+        /**
+         * @brief 控制循环（定时器回调）
+         * 
+         * 在控制循环中：
+         * - 检查并处理停止请求
+         * - 执行正在进行的动作（MoveHome, MoveLeg, MoveWaist, MoveWheel）
+         * - 发布关节轨迹命令
+         */
+        void ControlLoop();
+
+    private:
+        // ==================== ROS 2 通信接口 ====================
+        
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ethercatSetPub_;     ///< EtherCAT设置发布器
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;   ///< 手柄命令订阅器
+        rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;  ///< 关节状态订阅器
+        rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;                     ///< 电机命令发布器
+        rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;               ///< 轮子状态订阅器
+        rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;                      ///< IMU消息订阅器
+        rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;                 ///< 电机参数客户端
+
+        // ==================== 核心组件 ====================
+        
+        RobotControlManager robotControlManager_;              ///< 机器人控制管理器
+        std::unique_ptr<ActionManager> action_manager_;        ///< 动作管理器
+
+        // ==================== 数据记录 ====================
+        
+        std::ofstream data_file_;      ///< 数据文件流（用于记录关节数据）
+        std::string data_file_path_;   ///< 数据文件路径
+
+        // ==================== 定时器和时间 ====================
+        
+        rclcpp::TimerBase::SharedPtr controlTimer_;  ///< 控制循环定时器
+        rclcpp::Time lastTime_;                      ///< 上次控制循环时间
+
+        // ==================== 控制状态 ====================
+        
+        bool isStopping_;    ///< 是否正在停止
+        bool isJogMode_;     ///< 是否处于点动模式
+        int jogDirection_;   ///< 点动方向（-1, 0, 1）
+        size_t jogIndex_;    ///< 当前点动的关节索引
+        double jogValue_;    ///< 点动值
+        double lastSpeed_;   ///< 上次速度
+
+        // ==================== 回调函数 ====================
+        
+        /**
+         * @brief 发布关节轨迹
+         */
+        void Publish_joint_trajectory();
+
+        /**
+         * @brief 关节状态回调函数
+         * @param msg 关节状态消息
+         */
+        void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
+        
+        /**
+         * @brief 手柄命令回调函数
+         * @param msg 手柄命令消息
+         */
+        void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
+        
+        /**
+         * @brief 轮子状态回调函数
+         * @param cmd_msg 电机位置消息
+         */
+        void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
+        
+        /**
+         * @brief IMU消息回调函数
+         * @param msg IMU消息
+         */
+        void ImuMsgCallback(const ImuMsg::SharedPtr msg);
+    };
+}
--- a/src/robot_control/include/leg_control.hpp
+++ b/src/robot_control/include/leg_control.hpp
@@ -1,40 +0,0 @@
-#pragma once
-
-#include "rclcpp/rclcpp.hpp"
-#include "trapezoidal_trajectory.hpp"
-#include "control_base.hpp"
-
-namespace robot_control
-{
-    /**
-     * @brief LegControl 类用于控制腿部运动
-     * 
-     * 该类提供了腿部关节的控制功能，包括关节重置、移动和计算目标位置等操作。
-     */
-    class LegControl : public ControlBase
-    {
-        public:
-            LegControl(
-                size_t total_joints, 
-                const std::vector<double>& lengthParameters, 
-                const std::vector<double>& maxSpeed,
-                const std::vector<double>& maxAcc,
-                const std::vector<double>& minLimits, 
-                const std::vector<double>& maxLimits, 
-                const std::vector<double>& home_positions,
-                const std::vector<double>& zero_positions,
-                const std::vector<int>& joint_directions
-            );
-
-            ~LegControl() override;
-
-            bool SetMoveLegParametersInternal(double moveDistance);
-
-            double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree = false);
-
-            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
-
-            bool MoveUp(std::vector<double>& joint_positions, double dt);
-    };
-
-}  // namespace robot_control
--- a/src/robot_control/include/motion_parameters.hpp
+++ b/src/robot_control/include/motion_parameters.hpp
@@ -1,320 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <iostream>
-#include <map>
-#include <string>
-
-
-#define CYCLE_TIME 8    // 插补周期（毫秒）
-
-#ifndef DEG2RAD
-#define DEG2RAD(x) ((x)*0.017453293)
-#endif
-
-#ifndef RAD2DEG
-#define RAD2DEG(x) ((x)*57.29578)
-#endif
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-namespace robot_control 
-{
-    enum class LimitType {
-        POSITION,   // 位置限制（单位：度/弧度）
-        VELOCITY,   // 速度限制（单位：度/秒/弧度/秒）
-        EFFORT      // 力矩限制（单位：N·m）
-    };
-
-    // 单个关节的限制参数：包含 max（最大值）、min（最小值）、限制类型
-    struct JointLimit {
-        int index;          // 关节索引
-        double max;          // 关节运动范围上限（如位置最大角度）
-        double min;          // 关节运动范围下限（如位置最小角度）
-        LimitType limit_type;// 限制类型（区分位置/速度/力矩）
-
-        // 构造函数：默认初始化（避免未定义值）
-        JointLimit() : index(0), max(0.0), min(0.0), limit_type(LimitType::POSITION) {}
-
-        // 带参构造函数：快速初始化
-        JointLimit(int i, double max_val, double min_val, LimitType type) 
-            :index(i), max(max_val), min(min_val), limit_type(type) {
-            if (max < min) {
-                std::cerr << "[Warning] JointLimit: max (" << max << ") < min (" << min << "), swapping values!" << std::endl;
-                std::swap(max, min);
-            }
-        }
-    };
-
-    class MotionParameters 
-    {
-        public:
-            MotionParameters()
-            {
-                // 初始化结构参数 unit m
-                //TODO: 修改为实际参数
-                wheel_radius_ = 0.09;
-                wheel_separation_ = 0.55;
-
-                wheel_length_ = {
-                    wheel_radius_,
-                    wheel_separation_
-                };
-
-                // 腿长参数 unit m
-                leg_length_ = {
-                    0.66,
-                    0.385,
-                    0.362,
-                    0.55,
-                    0.4
-                };
-
-                waist_length_ = {
-                    0.1,
-                };
- 
-                // 轮子速度参数
-                max_linear_speed_x_ = 100;
-                max_linear_speed_z_ = 10;
-                max_angular_speed_z_ = 50;
-
-                // 关节速度参数
-                max_joint_velocity_ = {
-                    30,
-                    30,
-                    50,
-                    50,
-                    50,
-                    50,
-                    50,
-                    50
-                }; 
-
-                max_joint_acceleration_ = {
-                    80,
-                    80,
-                    100,
-                    100,
-                    100,
-                    100,
-                    100,
-                    100
-                };
-
-                // 初始化关节索引
-                wheel_joint_indices_ = {0, 1}; 
-
-                waist_joint_indices_ = {0, 1}; 
-
-                leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
-
-                total_joints_ = 8; 
-
-                // 初始化关节方向
-                wheel_joint_directions_ = {1, -1};  
-
-                waist_joint_directions_ = {1, 1};
-
-                leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};  
-
-                //TODO: check the ratio
-                joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比 都一样
-                
-                joint_resolution_ = {524288, 524288, 524288 , 524288, 524288, 524288, 524288, 524288};  // 分辨率 都一样
-
-
-                //TODO: 限位需要修改
-                joint_limits_ = {
-                    JointLimit(0, 40.0,     -40.0, LimitType::POSITION),
-                    JointLimit(1, 190.0,    -190.0, LimitType::POSITION),
-                    JointLimit(2, 0.0,      -90.0, LimitType::POSITION),
-                    JointLimit(3, 90.0,     0.0, LimitType::POSITION),
-                    JointLimit(4, 60,       0.0, LimitType::POSITION),
-                    JointLimit(5, 0.0,      -90.0, LimitType::POSITION),
-                    JointLimit(6, 0.0,      -60.0, LimitType::POSITION),
-                    JointLimit(7, 90.0,     0.0, LimitType::POSITION),
-                };
-
-                // 初始化限制参数
-                wheel_max_velocity_.resize(wheel_joint_indices_.size());
-                wheel_max_acceleration_.resize(wheel_joint_indices_.size());
-                wheel_max_limit_.resize(wheel_joint_indices_.size());
-                wheel_min_limit_.resize(wheel_joint_indices_.size());
-
-                wheel_max_velocity_ = {
-                    5,
-                    5
-                };
-                wheel_max_acceleration_ = {
-                    25,
-                    25
-                };
-
-                //There is no limit for wheel
-                wheel_max_limit_ = {
-                    0.0,
-                    0.0
-                };
-                wheel_min_limit_ = {
-                    0.0,
-                    0.0
-                };
-
-                waist_min_limit_.resize(waist_joint_indices_.size());
-                waist_max_limit_.resize(waist_joint_indices_.size());
-                waist_max_velocity_.resize(waist_joint_indices_.size());
-                waist_max_acceleration_.resize(waist_joint_indices_.size());
-
-                leg_max_limit_.resize(leg_joint_indices_.size());
-                leg_min_limit_.resize(leg_joint_indices_.size());
-                leg_max_velocity_.resize(leg_joint_indices_.size());
-                leg_max_acceleration_.resize(leg_joint_indices_.size());
-                
-                for (size_t i = 0; i < waist_joint_indices_.size(); i++)
-                {
-                    waist_max_limit_[i] = joint_limits_[waist_joint_indices_[i]].max;
-                    waist_min_limit_[i] = joint_limits_[waist_joint_indices_[i]].min;
-                    waist_max_velocity_[i] = max_joint_velocity_[waist_joint_indices_[i]];
-                    waist_max_acceleration_[i] = max_joint_acceleration_[waist_joint_indices_[i]];
-                }
-
-                for (size_t i = 0; i < leg_joint_indices_.size(); i++)
-                {
-                    leg_max_limit_[i] = joint_limits_[leg_joint_indices_[i]].max;
-                    leg_min_limit_[i] = joint_limits_[leg_joint_indices_[i]].min;
-                    leg_max_velocity_[i] = max_joint_velocity_[leg_joint_indices_[i]];
-                    leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
-                }
-                
-                waist_zero_positions_ = {
-                    350.0,
-                    261.82
-                };
-
-                leg_zero_positions_ = {
-                    217.52,        //316786.46
-                    120.84,        //175986.005
-                    108.7,         //158305.849
-                    221.95,        //323238.116
-                    234.14,        //340991.09
-                    125.39         //182612.423
-                };
-
-                waist_home_positions_ = {
-                    350.0,
-                    261.82
-                };
-
-
-                // 后腿零点为站直时候的状态, 然后 + 25度, 后腿长 0.66m, 25度为 0.6m
-                // 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36  膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
-                // 初始化初始位置
-                leg_home_positions_ = {
-                    217.52 - 65.0, 
-                    120.84 + 41.0, 
-                    108.7 + 40.63,   //217479
-                    221.95 - 41.0, 
-                    234.14 - 29.504,  //298023 
-                    125.39 + 65.0
-                };
-
-                // leg_home_positions_ = {
-                //     217.52 - 90.0,      //185.714.46
-                //     120.84 + 90.0,      //307058
-                //     108.7 + 0.0,        //158305
-                //     221.95 - 90.0,      //192166
-                //     234.14 - 0.0,       //340991
-                //     125.39 + 90.0       //313684
-                // };
-
-                // 初始化零点位置
-                wheel_home_positions_ = {
-                    0.0,
-                    0.0
-                };
-
-                wheel_zero_positions_ = {
-                    0.0,
-                    0.0
-                };
-
-                pulse_to_degree_.resize(joint_resolution_.size());
-                degree_to_pulse_.resize(joint_resolution_.size());
-
-                for (size_t i = 0; i < joint_resolution_.size(); i++)
-                {
-                    degree_to_pulse_[i] = joint_resolution_[i] / 360.0;
-                    pulse_to_degree_[i] = 360.0 / joint_resolution_[i];
-                }
-
-                jog_step_size_ = 10.0/ (1000.0 / CYCLE_TIME) ;     // 5度每秒
-
-            };
-
-            // 运动参数
-            size_t total_joints_;           // 总关节数
-
-            // 关节索引
-            std::vector<int> leg_joint_indices_;    // 腿部关节索引
-            std::vector<int> wheel_joint_indices_;  // 轮子关节索引
-            std::vector<int> waist_joint_indices_;   // 腰部关节索引
-            std::vector<int> real_waist_joint_indices_;  // 实际腰部关节索引
-            std::vector<int> real_leg_joint_indices_;     // 实际腿部关节索引
-
-            std::vector<int> wheel_joint_directions_; // 轮子关节方向
-            std::vector<int> waist_joint_directions_; // 身体关节方向
-            std::vector<int> leg_joint_directions_; // 腿部关节方向
-
-            std::vector<double> leg_home_positions_; // 左腿初始位置
-            std::vector<double> waist_home_positions_; // 身体初始位置
-            std::vector<double> wheel_home_positions_; // 轮子零点位置
-
-            std::vector<double> waist_zero_positions_; // 身体零点位置
-            std::vector<double> leg_zero_positions_; // 左腿零点位置
-            std::vector<double> wheel_zero_positions_; // 轮子零点位置
-
-            // 限制参数
-            std::vector<JointLimit> joint_limits_; // 关节限制
-            std::vector<double> waist_min_limit_;
-            std::vector<double> waist_max_limit_;
-            std::vector<double> leg_min_limit_;
-            std::vector<double> leg_max_limit_;
-            std::vector<double> wheel_min_limit_;
-            std::vector<double> wheel_max_limit_;
-
-            std::vector<double> max_joint_velocity_;
-            std::vector<double> waist_max_velocity_;
-            std::vector<double> leg_max_velocity_;
-            std::vector<double> wheel_max_velocity_;
-
-            std::vector<double> max_joint_acceleration_;
-            std::vector<double> waist_max_acceleration_;
-            std::vector<double> leg_max_acceleration_;
-            std::vector<double> wheel_max_acceleration_;
-
-            // 轮子控制相关
-            double wheel_radius_;         // 轮子半径（米）
-            double wheel_separation_;     // 轮距（米）
-            double max_linear_speed_x_;       // 线速度（m/s）
-            double max_linear_speed_z_;       // 线速度（m/s）
-            double max_angular_speed_z_;      // 角速度（rad/s）
-
-            // 尺寸相关
-            std::vector<double> leg_length_; 
-            std::vector<double> waist_length_;
-            std::vector<double> wheel_length_;
-
-            //关节传动比和分辨率
-            std::vector<double> joint_gear_ratio_;
-            std::vector<double> joint_resolution_;
-            
-            std::vector<double> degree_to_pulse_; // 角度转脉冲的转换因子
-            std::vector<double> pulse_to_degree_; // 脉冲转角度的转换因子
-
-            double jog_step_size_; // Jog步长
-
-    };
-}
--- a/src/robot_control/include/robot_control_manager.hpp
+++ b/src/robot_control/include/robot_control_manager.hpp
@@ -1,127 +0,0 @@
-#pragma once
-
-#include <cmath>
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
-#include "geometry_msgs/msg/twist.hpp"
-#include "sensor_msgs/msg/joint_state.hpp"
-#include "sensor_msgs/msg/imu.hpp"
-#include "motion_parameters.hpp"
-#include "arm_control.hpp"
-#include "leg_control.hpp"
-#include "wheel_control.hpp"
-#include "robot_model.hpp"
-#include "common_enum.hpp"
-#include "waist_control.hpp"
-
-#include "interfaces/msg/motor_pos.hpp"
-#include "interfaces/msg/imu_msg.hpp"
-
-using MotorPos = interfaces::msg::MotorPos;
-using ImuMsg = sensor_msgs::msg::Imu;
-
-namespace robot_control
-{
-    class RobotControlManager
-    {
-        public:
-            RobotControlManager();
-            ~RobotControlManager();
-
-            // 控制机器人运动
-            bool MoveLeg(double dt);
-            bool MoveWaist(double dt);
-            bool MoveWheel();
-            bool Stop(double dt);
-            bool GoHome(double dt);
-            void JogAxis(size_t axis, int dir);
-            void SetJogWheel(bool value);
-            bool GetJogWheel();
-
-            void WheelReset(){isWheelHomed_ = false;};
-            void ImuReset(){isGyroInited_ = false;}
-
-            // 检查参数是否合理
-            bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
-            bool SetMoveLegParameters(double moveLegDistance);
-            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
-            
-            // 机器人关节指令
-            std_msgs::msg::Float64MultiArray GetJointCommands();
-            std_msgs::msg::Float64MultiArray GetJointFeedback();
-            std_msgs::msg::Float64MultiArray GetWheelCommands();
-            std_msgs::msg::Float64MultiArray GetWheelFeedback();
-            double GetWheelRatio();
-
-            // 机器人状态
-            void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
-            void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
-            void UpdateWheelStates(const MotorPos::SharedPtr msg);
-            void UpdateImuMsg(const ImuMsg::SharedPtr msg);
-
-            MotionParameters GetMotionParameters();
-
-            std::vector<double> GetImuDifference();
-
-            bool IsMoving(MovementPart part);
-
-            bool RobotInitFinished();
-            
-        private:
-            void init();
-
-            MotionParameters motionParams_;
-
-            bool isWaistHomed_;
-            bool isLegHomed_;
-            bool isWheelHomed_;
-
-            bool is_wheel_jog_;
-
-            // 控制器
-            LegControl* legController_;
-            WheelControl* wheelController_;
-            WaistControl* waistController_;
-            
-            // 运动状态
-            std::vector<double> wheelPositions_;
-            std::vector<double> jointPositions_; // 关节位置（弧度）
-
-            std::vector<double> jointVelocities_; // 关节速度（弧度/秒）
-            std::vector<double> wheelVelocities_; // 关节速度（弧度/秒）
-
-            std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
-            std::vector<double> wheelAccelerations_; // 关节加速度（弧度/秒^2）
-
-            std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
-            std::vector<double> wheelEfforts_; // 关节力矩（牛顿米）
-
-            std::vector<bool> jointInited_; // 机器人是否已经初始化
-            bool isJointInitValueSet_;
-
-            // 陀螺仪状态
-            ImuMsg imuMsg_;
-            bool isGyroInited_;
-            std::vector<double> gyroValues_; // 陀螺仪数据（弧度/秒）
-            std::vector<double> gyroInitValues_; // 陀螺仪初始位置
-            std::vector<double> gyroVelocities_; // 陀螺仪速度（弧度/秒）
-            std::vector<double> gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2）
-            
-            // 临时变量
-            std::vector<double> tempWaistCmd_;
-            std::vector<double> tempLegCmd_;
-            std::vector<double> tempWheelCmd_;
-
-            MotorPos motorPos_;
-            sensor_msgs::msg::JointState jointStates_;
-            std_msgs::msg::Float64MultiArray jointCommands_; 
-            std_msgs::msg::Float64MultiArray wheelCommands_;
-
-            void AssignTempValues();
-            void UpdateJointCommands();
-
-            void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
-            void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
-    };
-
-} // namespace robot_control_manager
--- a/src/robot_control/include/robot_control_node.hpp
+++ b/src/robot_control/include/robot_control_node.hpp
@@ -1,117 +0,0 @@
-#pragma once
-
-#include <filesystem>
-#include <fstream>  // 添加这行来支持文件流操作
-#include <time.h>
-#include <rclcpp_action/rclcpp_action.hpp>
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
-#include "trajectory_msgs/msg/joint_trajectory.hpp"
-#include "std_msgs/msg/string.hpp"
-#include "robot_control_manager.hpp"
-
-#include "interfaces/action/move_home.hpp"
-#include "interfaces/action/move_leg.hpp"
-#include "interfaces/action/move_waist.hpp"
-#include "interfaces/action/move_wheel.hpp"
-#include "interfaces/msg/motor_cmd.hpp"
-#include "interfaces/srv/motor_param.hpp"
-
-#define RECORD_FLAG 0
-
-using MoveHome = interfaces::action::MoveHome;
-using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
-
-using MoveLeg = interfaces::action::MoveLeg;
-using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
-
-using MoveWaist = interfaces::action::MoveWaist;
-using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
-
-using MoveWheel = interfaces::action::MoveWheel;
-using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
-
-using MotorCmd = interfaces::msg::MotorCmd;
-
-using MotorParam = interfaces::srv::MotorParam;
-
-namespace robot_control
-{
-    class RobotControlNode : public rclcpp::Node 
-    {
-        public:
-            RobotControlNode();
-            ~RobotControlNode();
-            
-            // 状态机主循环
-            void ControlLoop();
-
-            rclcpp_action::GoalResponse handle_move_home_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveHome::Goal> goal);
-            rclcpp_action::CancelResponse handle_move_home_cancel(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
-            void handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
-            void move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
-            
-            rclcpp_action::GoalResponse handle_move_leg_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveLeg::Goal> goal);
-            rclcpp_action::CancelResponse handle_move_leg_cancel(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
-            void handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
-            void move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
-            
-            rclcpp_action::GoalResponse handle_move_waist_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveWaist::Goal> goal);
-            rclcpp_action::CancelResponse handle_move_waist_cancel(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
-            void handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
-            void move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
-
-            rclcpp_action::GoalResponse handle_move_wheel_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveWheel::Goal> goal);
-            rclcpp_action::CancelResponse handle_move_wheel_cancel(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
-            void handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
-            void move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
-
-        private:
-
-            rclcpp_action::Server<MoveHome>::SharedPtr move_home_action_server_;
-            bool move_home_executing_;
-            rclcpp_action::Server<MoveLeg>::SharedPtr move_leg_action_server_;
-            bool move_leg_executing_;
-            rclcpp_action::Server<MoveWaist>::SharedPtr move_waist_action_server_;
-            bool move_waist_executing_;
-            rclcpp_action::Server<MoveWheel>::SharedPtr move_wheel_action_server_;
-            bool move_wheel_executing_;
-
-            bool isStopping_;
-
-            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ethercatSetPub_;
-            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;
-            rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
-            rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
-            rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
-            rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;
-            rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;
-
-            RobotControlManager robotControlManager_;
-
-             // 文件流相关
-            std::ofstream data_file_;  // 用于写入数据的文件流
-            std::string data_file_path_;  // 数据文件路径
-
-            rclcpp::TimerBase::SharedPtr controlTimer_;
-            rclcpp::Time lastTime_;  // 移至类成员
-
-            bool isJogMode_;
-
-            int jogDirection_;
-            size_t jogIndex_;
-            double jogValue_;
-
-            double lastSpeed_;
-
-            void Publish_joint_trajectory();
-
-            void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
-            void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
-            void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
-            void ImuMsgCallback(const ImuMsg::SharedPtr msg);
-    };
-
-} // namespace robot_control
-
--- a/src/robot_control/include/utils/deceleration_planner.hpp
+++ b/src/robot_control/include/utils/deceleration_planner.hpp
--- a/src/robot_control/include/utils/extended_kalman_filter.hpp
+++ b/src/robot_control/include/utils/extended_kalman_filter.hpp
--- a/src/robot_control/include/utils/robot_kinematics.hpp
+++ b/src/robot_control/include/utils/robot_kinematics.hpp
@@ -1,6 +1,6 @@
 #pragma once

-#include "robot_model.hpp"
+#include "utils/robot_model.hpp"
 #include <Eigen/Dense>
 #include <vector>
 #include <memory>
--- a/src/robot_control/include/utils/robot_model.hpp
+++ b/src/robot_control/include/utils/robot_model.hpp
--- a/src/robot_control/include/utils/trapezoidal_trajectory.hpp
+++ b/src/robot_control/include/utils/trapezoidal_trajectory.hpp
--- a/src/robot_control/include/utils/urdf_parser.hpp
+++ b/src/robot_control/include/utils/urdf_parser.hpp
@@ -4,7 +4,7 @@
 #include <memory>
 #include <urdf/model.h>

-#include "robot_model.hpp"
+#include "utils/robot_model.hpp"

 namespace robot_control
 {
--- a/src/robot_control/include/waist_control.hpp
+++ b/src/robot_control/include/waist_control.hpp
@@ -1,34 +0,0 @@
-#pragma once
-
-#include <Eigen/Dense>
-#include "control_base.hpp"  // 包含父类头文件
-
-namespace robot_control
-{
-    class WaistControl : public ControlBase
-    {
-        public:
-            WaistControl(
-                size_t total_joints, 
-                const std::vector<double>& lengthParameters, 
-                const std::vector<double>& maxSpeed,
-                const std::vector<double>& maxAcc,
-                const std::vector<double>& minLimits, 
-                const std::vector<double>& maxLimits, 
-                const std::vector<double>& home_positions,
-                const std::vector<double>& zero_positions,
-                const std::vector<int>& joint_directions
-            );
-
-            ~WaistControl() override;
-
-            // void SetHomePositions(const std::vector<double>& home_positions) override;
-
-            bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);
-
-            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
-
-            bool MoveWaist(std::vector<double>& joint_positions, double dt);
-    };
-
-}
--- a/src/robot_control/include/wheel_control.hpp
+++ b/src/robot_control/include/wheel_control.hpp
@@ -1,41 +0,0 @@
-#pragma once
-
-#include <Eigen/Dense>
-#include "control_base.hpp"
-
-namespace robot_control
-{
-    class WheelControl : public ControlBase
-    {
-        public:
-            WheelControl(
-                size_t total_joints, 
-                const std::vector<double>& lengthParameters, 
-                const std::vector<double>& maxSpeed,
-                const std::vector<double>& maxAcc,
-                const std::vector<double>& minLimits, 
-                const std::vector<double>& maxLimits, 
-                const std::vector<double>& home_positions,
-                const std::vector<double>& zero_positions,
-                const std::vector<int>& joint_directions
-            );
-
-            ~WheelControl() override;
-
-            double moveRatio_= 1.0;
-
-            double lastMoveDistance = 0.0;
-
-            double GetWheelRatioInternal(){ return moveRatio_ ;};
-
-            bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
-
-            bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle);
-
-            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
-
-            bool MoveWheel(std::vector<double>& joint_positions);
-
-            bool GoHome(std::vector<double>& joint_positions, double dt) override;
-    };
-} // namespace robot_control
--- a/src/robot_control/package.xml
+++ b/src/robot_control/package.xml
@@ -1,7 +1,6 @@
 <?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>robot_control</name>
  <version>1.0.0</version>
  <description>Robot control package</description>
  <maintainer email="Ray@email.com">Your Name</maintainer>
@@ -9,7 +8,6 @@

  <!-- 编译时依赖 -->
  <build_depend>ament_cmake</build_depend>
-  <build_depend>rosidl_default_generators</build_depend>
  <build_depend>action_msgs</build_depend>
  <build_depend>rclcpp</build_depend>  <!-- 编译时也需依赖，避免链接错误 -->
  <build_depend>rclcpp_action</build_depend>
@@ -32,8 +30,6 @@
  <exec_depend>nav_msgs</exec_depend>
  <exec_depend>interfaces</exec_depend>

-  <member_of_group>rosidl_interface_packages</member_of_group>
-
  <export>
    <build_type>ament_cmake</build_type>
  </export>
--- a/src/robot_control/scripts/plot_joint_trajectory_multi.py
+++ b/src/robot_control/scripts/plot_joint_trajectory_multi.py
@@ -48,18 +48,21 @@ def plot_joint_trajectories(file_path):
    plt.figure(figsize=(12, 8))
    for i in range(num_plots):
        plt.plot(joint_data[i], label=f'关节 {i+1}')
-
-    # 图形配置
-    plt.xlabel('时间 (秒)')
-    plt.ylabel('关节位置')
-    plt.title('所有关节轨迹曲线')
-    plt.grid(True, linestyle='--', alpha=0.7)
-    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')  # 图例放右侧
-    plt.tight_layout()  # 自动调整布局
+    
+    plt.xlabel('时间点索引')
+    plt.ylabel('关节值')
+    plt.title('关节轨迹图')
+    plt.legend()
+    plt.grid(True)
+    plt.tight_layout()
    plt.show()

 if __name__ == "__main__":
-    # 数据文件路径（与C++代码中的路径一致）
-    data_file = "/home/demo/ros2_joint_data.txt"
-    plot_joint_trajectories(data_file)
-    
+    import sys
+    if len(sys.argv) < 2:
+        print("用法: python plot_joint_trajectory_multi.py <数据文件路径>")
+        sys.exit(1)
+    
+    file_path = sys.argv[1]
+    plot_joint_trajectories(file_path)
+
--- a/src/robot_control/src/actions/action_manager.cpp
+++ b/src/robot_control/src/actions/action_manager.cpp
@@ -0,0 +1,471 @@
+/**
+ * @file action_manager.cpp
+ * @brief Action管理器实现
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#include "actions/action_manager.hpp"
+#include "core/robot_control_manager.hpp"
+#include "core/common_enum.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+
+#include <thread>
+#include <cmath>
+#include <chrono>
+
+using namespace robot_control;
+
+ActionManager::ActionManager(
+    rclcpp::Node* node,
+    RobotControlManager& robot_control_manager,
+    std::function<bool()> is_jog_mode_func,
+    rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
+    rclcpp::Client<MotorParam>::SharedPtr motor_param_client)
+    : node_(node)
+    , robot_control_manager_(robot_control_manager)
+    , is_jog_mode_func_(is_jog_mode_func)
+    , motor_cmd_pub_(motor_cmd_pub)
+    , motor_param_client_(motor_param_client)
+    , move_home_executing_(false)
+    , move_leg_executing_(false)
+    , move_waist_executing_(false)
+    , move_wheel_executing_(false)
+{
+}
+
+ActionManager::~ActionManager()
+{
+}
+
+void ActionManager::initialize()
+{
+    using namespace std::placeholders;
+
+    // 创建 MoveHome Action 服务器
+    move_home_action_server_ = rclcpp_action::create_server<MoveHome>(
+        node_,
+        "MoveHome",
+        std::bind(&ActionManager::handle_move_home_goal, this, _1, _2),
+        std::bind(&ActionManager::handle_move_home_cancel, this, _1),
+        std::bind(&ActionManager::handle_move_home_accepted, this, _1));
+    RCLCPP_INFO(node_->get_logger(), "MoveHome action server is ready");
+
+    // 创建 MoveLeg Action 服务器
+    move_leg_action_server_ = rclcpp_action::create_server<MoveLeg>(
+        node_,
+        "MoveLeg",
+        std::bind(&ActionManager::handle_move_leg_goal, this, _1, _2),
+        std::bind(&ActionManager::handle_move_leg_cancel, this, _1),
+        std::bind(&ActionManager::handle_move_leg_accepted, this, _1));
+    RCLCPP_INFO(node_->get_logger(), "MoveLeg action server is ready");
+
+    // 创建 MoveWaist Action 服务器
+    move_waist_action_server_ = rclcpp_action::create_server<MoveWaist>(
+        node_,
+        "MoveWaist",
+        std::bind(&ActionManager::handle_move_waist_goal, this, _1, _2),
+        std::bind(&ActionManager::handle_move_waist_cancel, this, _1),
+        std::bind(&ActionManager::handle_move_waist_accepted, this, _1));
+    RCLCPP_INFO(node_->get_logger(), "MoveWaist action server is ready");
+
+    // 创建 MoveWheel Action 服务器
+    move_wheel_action_server_ = rclcpp_action::create_server<MoveWheel>(
+        node_,
+        "MoveWheel",
+        std::bind(&ActionManager::handle_move_wheel_goal, this, _1, _2),
+        std::bind(&ActionManager::handle_move_wheel_cancel, this, _1),
+        std::bind(&ActionManager::handle_move_wheel_accepted, this, _1));
+    RCLCPP_INFO(node_->get_logger(), "MoveWheel action server is ready");
+}
+
+// MoveHome Action 处理函数
+rclcpp_action::GoalResponse ActionManager::handle_move_home_goal(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const MoveHome::Goal> goal)
+{
+    (void)uuid;
+    (void)goal;
+
+    if (robot_control_manager_.IsMoving(MovementPart::ALL))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Robot is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_home_executing_.load())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Another move home goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (is_jog_mode_func_())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse ActionManager::handle_move_home_cancel(
+    const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+{
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void ActionManager::handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+{
+    move_home_executing_.store(true);
+    using namespace std::placeholders;
+    std::thread{std::bind(&ActionManager::move_home_execute, this, _1), goal_handle}.detach();
+}
+
+void ActionManager::move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+{
+    rclcpp::Rate loop_rate(10);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<MoveHome::Feedback>();
+    auto result = std::make_shared<MoveHome::Result>();
+
+    while (move_home_executing_.load() && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(node_->get_logger(), "Move home canceled");
+            move_home_executing_.store(false);
+            return;
+        }
+
+        auto joint_commands = robot_control_manager_.GetJointFeedback();
+        feedback->joint_values.clear();
+        for (double val : joint_commands.data)
+        {
+            feedback->joint_values.push_back(static_cast<int64_t>(val));
+        }
+        goal_handle->publish_feedback(feedback);
+        loop_rate.sleep();
+    }
+
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(node_->get_logger(), "move home succeeded.");
+    }
+}
+
+// MoveLeg Action 处理函数
+rclcpp_action::GoalResponse ActionManager::handle_move_leg_goal(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const MoveLeg::Goal> goal)
+{
+    (void)uuid;
+
+    if (robot_control_manager_.IsMoving(MovementPart::LEG))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Robot leg is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_leg_executing_.load())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Another move leg goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (is_jog_mode_func_())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (!robot_control_manager_.SetMoveLegParameters(goal->move_up_distance))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to set move leg parameters");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse ActionManager::handle_move_leg_cancel(
+    const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+{
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void ActionManager::handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+{
+    move_leg_executing_.store(true);
+    using namespace std::placeholders;
+    std::thread{std::bind(&ActionManager::move_leg_execute, this, _1), goal_handle}.detach();
+}
+
+void ActionManager::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+{
+    rclcpp::Rate loop_rate(10);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<MoveLeg::Feedback>();
+    auto result = std::make_shared<MoveLeg::Result>();
+
+    while (move_leg_executing_.load() && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(node_->get_logger(), "move leg canceled");
+            move_leg_executing_.store(false);
+            return;
+        }
+
+        auto joint_commands = robot_control_manager_.GetJointFeedback();
+        feedback->joint_values.clear();
+        for (double val : joint_commands.data)
+        {
+            feedback->joint_values.push_back(static_cast<int64_t>(val));
+        }
+        goal_handle->publish_feedback(feedback);
+        loop_rate.sleep();
+    }
+
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(node_->get_logger(), "move leg succeeded.");
+    }
+}
+
+// MoveWaist Action 处理函数
+rclcpp_action::GoalResponse ActionManager::handle_move_waist_goal(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const MoveWaist::Goal> goal)
+{
+    (void)uuid;
+
+    if (robot_control_manager_.IsMoving(MovementPart::WAIST))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Robot waist is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_waist_executing_.load())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Another move waist goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (is_jog_mode_func_())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (!robot_control_manager_.SetMoveWaistParameters(goal->move_pitch_degree, goal->move_yaw_degree))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Invalid move waist request");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse ActionManager::handle_move_waist_cancel(
+    const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+{
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void ActionManager::handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+{
+    move_waist_executing_.store(true);
+    using namespace std::placeholders;
+    std::thread{std::bind(&ActionManager::move_waist_execute, this, _1), goal_handle}.detach();
+}
+
+void ActionManager::move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+{
+    rclcpp::Rate loop_rate(10);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<MoveWaist::Feedback>();
+    auto result = std::make_shared<MoveWaist::Result>();
+
+    while (move_waist_executing_.load() && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(node_->get_logger(), "move waist canceled");
+            move_waist_executing_.store(false);
+            return;
+        }
+
+        auto joint_commands = robot_control_manager_.GetJointFeedback();
+        feedback->joint_values.clear();
+        for (double val : joint_commands.data)
+        {
+            feedback->joint_values.push_back(static_cast<int64_t>(val));
+        }
+        goal_handle->publish_feedback(feedback);
+        loop_rate.sleep();
+    }
+
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(node_->get_logger(), "move waist succeeded.");
+    }
+}
+
+// MoveWheel Action 处理函数
+rclcpp_action::GoalResponse ActionManager::handle_move_wheel_goal(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const MoveWheel::Goal> goal)
+{
+    (void)uuid;
+
+    if (robot_control_manager_.IsMoving(MovementPart::WHEEL))
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Robot wheel is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_wheel_executing_.load())
+    {
+        RCLCPP_ERROR(node_->get_logger(), "Another move wheel goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (goal->move_distance > 2.1 || goal->move_distance < -1.0 || goal->move_angle < -10 || goal->move_angle > 10)
+    {
+        RCLCPP_ERROR(node_->get_logger(), "exceed limit");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse ActionManager::handle_move_wheel_cancel(
+    const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
+{
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void ActionManager::handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
+{
+    move_wheel_executing_.store(true);
+    RCLCPP_INFO(node_->get_logger(), "Goal accepted");
+    using namespace std::placeholders;
+    std::thread{std::bind(&ActionManager::move_wheel_execute, this, _1), goal_handle}.detach();
+}
+
+void ActionManager::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
+{
+    // 注意：这里需要从原始的 robot_control_node.cpp 中的 move_wheel_execute 函数复制完整实现
+    // 由于代码较长且涉及 motorCmdPub_ 和 motorParamClient_，这里提供框架
+    // 完整实现需要从原文件中迁移
+
+    rclcpp::Rate loop_rate(10);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<MoveWheel::Feedback>();
+    auto result = std::make_shared<MoveWheel::Result>();
+
+    double wheelAngle = 0;
+
+    if (abs(goal->move_angle) > 0)
+    {
+        robot_control_manager_.SetJogWheel(true);
+        wheelAngle = goal->move_angle;
+    }
+
+    if (abs(goal->move_angle) == 0 && abs(goal->move_distance) == 0)
+    {
+        robot_control_manager_.SetJogWheel(false);
+    }
+
+    if (!robot_control_manager_.GetJogWheel())
+    {
+        double tempValue = robot_control_manager_.GetImuDifference()[2];
+        wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
+    }
+
+    robot_control_manager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
+    double ratio = robot_control_manager_.GetWheelRatio();
+
+    auto request = std::make_shared<MotorParam::Request>();
+
+    if ((goal->move_distance > 0.1) && !robot_control_manager_.GetJogWheel())
+    {
+        request->motor_id = 1;
+        request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
+        request->add_acc = 8;
+        request->dec_acc = 8;
+        motor_param_client_->async_send_request(request);
+        std::cout << "set motor 1 max speed : " << request->max_speed << std::endl;
+        rclcpp::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    robot_control_manager_.MoveWheel();
+
+    std_msgs::msg::Float64MultiArray wheel_commands = robot_control_manager_.GetWheelCommands();
+
+    MotorCmd wheel_commands_msg;
+    wheel_commands_msg.target = "rs485";
+    wheel_commands_msg.type = "bm";
+    wheel_commands_msg.position = "";
+    wheel_commands_msg.motor_id = {1, 2};
+    wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]), (float)(wheel_commands.data[1])};
+
+    motor_cmd_pub_->publish(wheel_commands_msg);
+
+    while (move_wheel_executing_.load() && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(node_->get_logger(), "move wheel canceled");
+            move_wheel_executing_.store(false);
+            return;
+        }
+
+        auto joint_feedback = robot_control_manager_.GetWheelFeedback();
+
+        if (abs(joint_feedback.data[0] - wheel_commands.data[0]) < 20.0 &&
+            abs(joint_feedback.data[1] - wheel_commands.data[1]) < 20.0)
+        {
+            move_wheel_executing_.store(false);
+        }
+
+        feedback->current_pos = joint_feedback.data[0];
+        goal_handle->publish_feedback(feedback);
+        loop_rate.sleep();
+    }
+
+    if ((goal->move_distance > 0.0) && !robot_control_manager_.GetJogWheel())
+    {
+        request->motor_id = 1;
+        request->max_speed = 51;
+        request->add_acc = 8;
+        request->dec_acc = 8;
+        motor_param_client_->async_send_request(request);
+    }
+
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(node_->get_logger(), "move wheel succeeded.");
+    }
+}
+
--- a/src/robot_control/src/controllers/arm_control.cpp
+++ b/src/robot_control/src/controllers/arm_control.cpp
@@ -1,4 +1,4 @@
-#include "arm_control.hpp"
+#include "controllers/arm_control.hpp"
 #include <stdexcept>
 #include <vector>

--- a/src/robot_control/src/controllers/control_base.cpp
+++ b/src/robot_control/src/controllers/control_base.cpp
@@ -1,4 +1,4 @@
-#include "control_base.hpp"
+#include "controllers/control_base.hpp"
 #include <stdexcept>
 #include <vector>

--- a/src/robot_control/src/controllers/leg_control.cpp
+++ b/src/robot_control/src/controllers/leg_control.cpp
@@ -1,4 +1,4 @@
-#include "leg_control.hpp"
+#include "controllers/leg_control.hpp"
 #include <stdexcept>
 #include <vector>

--- a/src/robot_control/src/controllers/waist_control.cpp
+++ b/src/robot_control/src/controllers/waist_control.cpp
@@ -1,4 +1,4 @@
-#include "waist_control.hpp"
+#include "controllers/waist_control.hpp"
 #include <stdexcept>
 #include <vector>

--- a/src/robot_control/src/controllers/wheel_control.cpp
+++ b/src/robot_control/src/controllers/wheel_control.cpp
@@ -1,4 +1,4 @@
-#include "wheel_control.hpp"
+#include "controllers/wheel_control.hpp"
 #include <vector>

 namespace robot_control
--- a/src/robot_control/src/core/controller_factory.cpp
+++ b/src/robot_control/src/core/controller_factory.cpp
@@ -0,0 +1,139 @@
+/**
+ * @file controller_factory.cpp
+ * @brief 控制器工厂实现
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#include "core/controller_factory.hpp"
+#include "controllers/control_base.hpp"
+#include "controllers/leg_control.hpp"
+#include "controllers/arm_control.hpp"
+#include "controllers/wheel_control.hpp"
+#include "controllers/waist_control.hpp"
+#include "core/motion_parameters.hpp"
+
+#include <algorithm>
+#include <cctype>
+
+using namespace robot_control;
+
+std::unique_ptr<ControlBase> ControllerFactory::create_controller(
+    ControllerType type,
+    const MotionParameters& motion_params)
+{
+    switch (type)
+    {
+        case ControllerType::LEG_CONTROLLER:
+            return std::make_unique<LegControl>(
+                motion_params.leg_joint_indices_.size(),
+                motion_params.leg_length_,
+                motion_params.leg_max_velocity_,
+                motion_params.leg_max_acceleration_,
+                motion_params.leg_min_limit_,
+                motion_params.leg_max_limit_,
+                motion_params.leg_home_positions_,
+                motion_params.leg_zero_positions_,
+                motion_params.leg_joint_directions_);
+
+        case ControllerType::ARM_CONTROLLER:
+            // ARM controller not currently implemented in MotionParameters
+            // Return nullptr or throw exception if ARM is requested but not configured
+            // TODO: Add ARM parameters to MotionParameters if needed
+            return nullptr;
+
+        case ControllerType::WHEEL_CONTROLLER:
+            return std::make_unique<WheelControl>(
+                motion_params.wheel_joint_indices_.size(),
+                motion_params.wheel_length_,
+                motion_params.wheel_max_velocity_,
+                motion_params.wheel_max_acceleration_,
+                motion_params.wheel_min_limit_,
+                motion_params.wheel_max_limit_,
+                motion_params.wheel_home_positions_,
+                motion_params.wheel_zero_positions_,
+                motion_params.wheel_joint_directions_);
+
+        case ControllerType::WAIST_CONTROLLER:
+            return std::make_unique<WaistControl>(
+                motion_params.waist_joint_indices_.size(),
+                motion_params.waist_length_,
+                motion_params.waist_max_velocity_,
+                motion_params.waist_max_acceleration_,
+                motion_params.waist_min_limit_,
+                motion_params.waist_max_limit_,
+                motion_params.waist_home_positions_,
+                motion_params.waist_zero_positions_,
+                motion_params.waist_joint_directions_);
+
+        default:
+            return nullptr;
+    }
+}
+
+std::string ControllerFactory::controller_type_to_string(ControllerType type)
+{
+    switch (type)
+    {
+        case ControllerType::LEG_CONTROLLER:
+            return "leg";
+        case ControllerType::ARM_CONTROLLER:
+            return "arm";
+        case ControllerType::WHEEL_CONTROLLER:
+            return "wheel";
+        case ControllerType::WAIST_CONTROLLER:
+            return "waist";
+        case ControllerType::UNKNOWN_CONTROLLER:
+            return "unknown";
+        default:
+            return "unknown";
+    }
+}
+
+ControllerType ControllerFactory::string_to_controller_type(const std::string& type_str)
+{
+    std::string lower_str = type_str;
+    std::transform(lower_str.begin(), lower_str.end(), lower_str.begin(), ::tolower);
+
+    if (lower_str == "leg")
+        return ControllerType::LEG_CONTROLLER;
+    else if (lower_str == "arm")
+        return ControllerType::ARM_CONTROLLER;
+    else if (lower_str == "wheel")
+        return ControllerType::WHEEL_CONTROLLER;
+    else if (lower_str == "waist")
+        return ControllerType::WAIST_CONTROLLER;
+    else
+        return ControllerType::UNKNOWN_CONTROLLER;
+}
+
+bool ControllerFactory::is_controller_enabled(
+    ControllerType type,
+    const std::vector<std::string>& enabled_controllers)
+{
+    // 使用 controller_type_to_string 将枚举转换为字符串
+    std::string type_str = controller_type_to_string(type);
+    
+    // 如果类型无效，返回false
+    if (type_str == "unknown")
+    {
+        return false;
+    }
+
+    // 转换为小写进行比较（虽然controller_type_to_string已经返回小写，但为了安全还是转换）
+    std::string lower_type_str = type_str;
+    std::transform(lower_type_str.begin(), lower_type_str.end(), lower_type_str.begin(), ::tolower);
+
+    for (const auto& enabled : enabled_controllers)
+    {
+        std::string lower_enabled = enabled;
+        std::transform(lower_enabled.begin(), lower_enabled.end(), lower_enabled.begin(), ::tolower);
+        if (lower_enabled == lower_type_str)
+        {
+            return true;
+        }
+    }
+
+    return false;
+}
+
--- a/src/robot_control/src/core/robot_control_manager.cpp
+++ b/src/robot_control/src/core/robot_control_manager.cpp
@@ -1,16 +1,23 @@
-#include "robot_control_manager.hpp"
+/**
+ * @file robot_control_manager.cpp
+ * @brief 机器人控制管理器实现
+ * @author Robot Control Team
+ * @date 2024
+ */
+
+#include "core/robot_control_manager.hpp"

 using namespace robot_control;
 using namespace std;
 using namespace std_msgs::msg;


-RobotControlManager::RobotControlManager()
+RobotControlManager::RobotControlManager(const std::vector<std::string>& enabled_controllers)
 {
-    this->init();
+    this->init(enabled_controllers);
 }

-void RobotControlManager::init()
+void RobotControlManager::init(const std::vector<std::string>& enabled_controllers)
 {
    jointInited_.resize(motionParams_.total_joints_, false);
    // jointInited_.resize(3, false);
@@ -33,41 +40,47 @@ void RobotControlManager::init()
        wheelCommands_.data.push_back(0.0);
    }

-    legController_ = new LegControl(
-        motionParams_.leg_joint_indices_.size(), 
-        motionParams_.leg_length_,
-        motionParams_.leg_max_velocity_,
-        motionParams_.leg_max_acceleration_,
-        motionParams_.leg_min_limit_,
-        motionParams_.leg_max_limit_,
-        motionParams_.leg_home_positions_,
-        motionParams_.leg_zero_positions_,
-        motionParams_.leg_joint_directions_
-    );
+    // 初始化控制器启用标志
+    leg_controller_enabled_ = false;
+    wheel_controller_enabled_ = false;
+    waist_controller_enabled_ = false;
+    arm_controller_enabled_ = false;

-    waistController_ = new WaistControl(
-        motionParams_.waist_joint_indices_.size(), 
-        motionParams_.waist_length_,
-        motionParams_.waist_max_velocity_,
-        motionParams_.waist_max_acceleration_,
-        motionParams_.waist_min_limit_,
-        motionParams_.waist_max_limit_,
-        motionParams_.waist_home_positions_,
-        motionParams_.waist_zero_positions_,
-        motionParams_.waist_joint_directions_
-    );
+    // 动态加载控制器
+    if (ControllerFactory::is_controller_enabled(ControllerType::LEG_CONTROLLER, enabled_controllers))
+    {
+        leg_controller_ = std::unique_ptr<LegControl>(
+            static_cast<LegControl*>(ControllerFactory::create_controller(
+                ControllerType::LEG_CONTROLLER, motionParams_).release()));
+        leg_controller_enabled_ = true;
+    }

-    wheelController_ = new WheelControl(
-        motionParams_.wheel_joint_indices_.size(), 
-        motionParams_.wheel_length_, 
-        motionParams_.wheel_max_velocity_,
-        motionParams_.wheel_max_acceleration_,
-        motionParams_.wheel_min_limit_,
-        motionParams_.wheel_max_limit_,
-        motionParams_.wheel_home_positions_,
-        motionParams_.wheel_zero_positions_,
-        motionParams_.wheel_joint_directions_
-    );
+    if (ControllerFactory::is_controller_enabled(ControllerType::WHEEL_CONTROLLER, enabled_controllers))
+    {
+        wheel_controller_ = std::unique_ptr<WheelControl>(
+            static_cast<WheelControl*>(ControllerFactory::create_controller(
+                ControllerType::WHEEL_CONTROLLER, motionParams_).release()));
+        wheel_controller_enabled_ = true;
+    }
+
+    if (ControllerFactory::is_controller_enabled(ControllerType::WAIST_CONTROLLER, enabled_controllers))
+    {
+        waist_controller_ = std::unique_ptr<WaistControl>(
+            static_cast<WaistControl*>(ControllerFactory::create_controller(
+                ControllerType::WAIST_CONTROLLER, motionParams_).release()));
+        waist_controller_enabled_ = true;
+    }
+
+    if (ControllerFactory::is_controller_enabled(ControllerType::ARM_CONTROLLER, enabled_controllers))
+    {
+        auto arm_ctrl = ControllerFactory::create_controller(ControllerType::ARM_CONTROLLER, motionParams_);
+        if (arm_ctrl)
+        {
+            arm_controller_ = std::unique_ptr<ArmControl>(
+                static_cast<ArmControl*>(arm_ctrl.release()));
+            arm_controller_enabled_ = true;
+        }
+    }

    isWaistHomed_ = false;
    isLegHomed_ = false;
@@ -79,9 +92,7 @@ void RobotControlManager::init()

 RobotControlManager::~RobotControlManager()
 {
-    delete waistController_;
-    delete legController_;
-    delete wheelController_;
+    // 智能指针会自动释放，无需手动 delete
 }

 void RobotControlManager::SetJogWheel(bool value)
@@ -98,20 +109,20 @@ bool RobotControlManager::SetMoveWheelParameters(double moveWheelDistance, doubl
 {
    if (is_wheel_jog_)
    {
-        return wheelController_->SetMoveWheelParametersInternalJog(moveWheelDistance, moveWheelAngle);
+        return wheel_controller_->SetMoveWheelParametersInternalJog(moveWheelDistance, moveWheelAngle);
    }

-    return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);
+    return wheel_controller_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);
 }

 bool RobotControlManager::SetMoveLegParameters(double moveLegDistance)
 {
-    return legController_->SetMoveLegParametersInternal(moveLegDistance);
+    return leg_controller_->SetMoveLegParametersInternal(moveLegDistance);
 }

 bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double moveYawAngle)
 {
-    return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
+    return waist_controller_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
 }

 Float64MultiArray RobotControlManager::GetWheelCommands()
@@ -149,7 +160,7 @@ Float64MultiArray RobotControlManager::GetWheelFeedback()

 double RobotControlManager::GetWheelRatio()
 {
-    return wheelController_->GetWheelRatioInternal();
+    return wheel_controller_ ? wheel_controller_->GetWheelRatioInternal() : 1.0;
 }

 Float64MultiArray RobotControlManager::GetJointCommands()
@@ -323,14 +334,16 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
        wheelEfforts_[i] = 0.0;
    }

-    if (!isWheelHomed_)
+    if (wheel_controller_)
    {
-        wheelController_ -> SetHomePositions(wheelPositions_);
-        isWheelHomed_ = true;
-    }
-    
+        if (!isWheelHomed_)
+        {
+            wheel_controller_->SetHomePositions(wheelPositions_);
+            isWheelHomed_ = true;
+        }

-    wheelController_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
+        wheel_controller_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
+    }
 }

 void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg) 
@@ -402,27 +415,33 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
        jointEfforts_.begin() + waistStartIndex, 
        jointEfforts_.begin() + waistStartIndex + waistJointSize
    );
-    waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
+    if (waist_controller_)
+    {
+        waist_controller_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
+    }

    // Update the leg controller
-    size_t legStartIndex = motionParams_.leg_joint_indices_[0];
-    size_t legJointSize = motionParams_.leg_joint_indices_.size();
+    if (leg_controller_)
+    {
+        size_t legStartIndex = motionParams_.leg_joint_indices_[0];
+        size_t legJointSize = motionParams_.leg_joint_indices_.size();

-    std::vector<double> legPositions(
-        jointPositions_.begin() + legStartIndex, 
-        jointPositions_.begin() + legStartIndex + legJointSize
-    );
+        std::vector<double> legPositions(
+            jointPositions_.begin() + legStartIndex, 
+            jointPositions_.begin() + legStartIndex + legJointSize
+        );

-    std::vector<double> legVelocities(
-        jointVelocities_.begin() + legStartIndex, 
-        jointVelocities_.begin() + legStartIndex + legJointSize
-    );
+        std::vector<double> legVelocities(
+            jointVelocities_.begin() + legStartIndex, 
+            jointVelocities_.begin() + legStartIndex + legJointSize
+        );

-    std::vector<double> legEfforts(
-        jointEfforts_.begin() + legStartIndex, 
-        jointEfforts_.begin() + legStartIndex + legJointSize
-    );
-    legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
+        std::vector<double> legEfforts(
+            jointEfforts_.begin() + legStartIndex, 
+            jointEfforts_.begin() + legStartIndex + legJointSize
+        );
+        leg_controller_->UpdateJointStates(legPositions, legVelocities, legEfforts);
+    }
 }

 MotionParameters RobotControlManager::GetMotionParameters()
@@ -435,13 +454,18 @@ bool RobotControlManager::IsMoving(MovementPart part)
    switch (part)
    {
        case MovementPart::WHEEL:
-            return wheelController_->IsMoving();
+            return wheel_controller_ ? wheel_controller_->IsMoving() : false;
        case MovementPart::LEG:
-            return legController_->IsMoving();
+            return leg_controller_ ? leg_controller_->IsMoving() : false;
        case MovementPart::WAIST:
-            return waistController_->IsMoving();
+            return waist_controller_ ? waist_controller_->IsMoving() : false;
        case MovementPart::ALL:
-            return wheelController_->IsMoving()  && waistController_->IsMoving() && legController_->IsMoving();
+            {
+                bool wheel_moving = wheel_controller_ ? wheel_controller_->IsMoving() : false;
+                bool waist_moving = waist_controller_ ? waist_controller_->IsMoving() : false;
+                bool leg_moving = leg_controller_ ? leg_controller_->IsMoving() : false;
+                return wheel_moving || waist_moving || leg_moving;  // 使用 OR，任一在运动就返回 true
+            }
        default:
            return false;
    }
@@ -456,9 +480,22 @@ bool RobotControlManager::Stop(double dt)
 {
    AssignTempValues();

-    bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
-    bool legStopped = legController_->Stop(tempLegCmd_, dt);
-    bool wheelStopped = wheelController_->Stop(tempWheelCmd_, dt);
+    bool waistStopped = true;
+    bool legStopped = true;
+    bool wheelStopped = true;
+
+    if (waist_controller_)
+    {
+        waistStopped = waist_controller_->Stop(tempWaistCmd_, dt);
+    }
+    if (leg_controller_)
+    {
+        legStopped = leg_controller_->Stop(tempLegCmd_, dt);
+    }
+    if (wheel_controller_)
+    {
+        wheelStopped = wheel_controller_->Stop(tempWheelCmd_, dt);
+    }

    UpdateJointCommands();

@@ -467,9 +504,14 @@ bool RobotControlManager::Stop(double dt)

 bool RobotControlManager::MoveLeg(double dt)
 {
+    if (!leg_controller_)
+    {
+        return false;
+    }
+
    AssignTempValues();

-    bool result = legController_ -> MoveUp(tempLegCmd_, dt);
+    bool result = leg_controller_->MoveUp(tempLegCmd_, dt);

    UpdateJointCommands();

@@ -478,9 +520,14 @@ bool RobotControlManager::MoveLeg(double dt)

 bool RobotControlManager::MoveWaist(double dt)
 {
+    if (!waist_controller_)
+    {
+        return false;
+    }
+
    AssignTempValues();

-    bool result = waistController_ -> MoveWaist(tempWaistCmd_, dt);
+    bool result = waist_controller_->MoveWaist(tempWaistCmd_, dt);

    UpdateJointCommands();

@@ -489,9 +536,14 @@ bool RobotControlManager::MoveWaist(double dt)

 bool RobotControlManager::MoveWheel()
 {
+    if (!wheel_controller_)
+    {
+        return false;
+    }
+
    tempWheelCmd_.resize(motionParams_.wheel_joint_indices_.size());

-    bool result = wheelController_ -> MoveWheel(tempWheelCmd_);
+    bool result = wheel_controller_->MoveWheel(tempWheelCmd_);

    for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
    {
@@ -506,14 +558,22 @@ bool RobotControlManager::GoHome(double dt)
 {
    AssignTempValues();

-    if (!isWaistHomed_)
+    if (waist_controller_ && !isWaistHomed_)
    {
-        isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
+        isWaistHomed_ = waist_controller_->GoHome(tempWaistCmd_, dt);
+    }
+    else if (!waist_controller_)
+    {
+        isWaistHomed_ = true;  // 如果没有控制器，认为已完成
    }

-    if (!isLegHomed_)
+    if (leg_controller_ && !isLegHomed_)
    {
-        isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
+        isLegHomed_ = leg_controller_->GoHome(tempLegCmd_, dt);
+    }
+    else if (!leg_controller_)
+    {
+        isLegHomed_ = true;  // 如果没有控制器，认为已完成
    }

    UpdateJointCommands();
--- a/src/robot_control/src/core/robot_control_node.cpp
+++ b/src/robot_control/src/core/robot_control_node.cpp
@@ -0,0 +1,305 @@
+#include <thread>
+#include "rclcpp/rclcpp.hpp"
+#include "core/robot_control_node.hpp"
+#include "core/motion_parameters.hpp"
+#include "core/common_enum.hpp"
+
+using namespace robot_control;
+
+using namespace std;
+using namespace std_msgs::msg;
+
+namespace fs = std::filesystem;
+
+RobotControlNode::RobotControlNode() : Node("robot_control_node")
+{
+    RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
+    isJogMode_ = false;
+    jogDirection_ = 0;
+    jogIndex_ = 0;
+    lastSpeed_ = 51;
+    isStopping_ = false;
+ 
+    // 初始化数据文件（设置路径，确保目录存在）
+    #if RECORD_FLAG
+    data_file_path_ = "/home/demo/ros2_joint_data.txt";  // 数据保存路径
+    fs::path file_path(data_file_path_);
+    fs::create_directories(file_path.parent_path());  // 自动创建父目录（如果不存在）
+    data_file_.open(data_file_path_, std::ios::trunc);
+    if (!data_file_.is_open()) {
+        
+    } else {
+        // 写入表头（仅在文件为空时）
+        data_file_.seekp(0, std::ios::end);  // 移动到文件末尾
+    }
+    #endif
+
+    // 创建发布器和客户端（ActionManager 需要）
+    motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
+    motorParamClient_ = this->create_client<MotorParam>("/motor_param");
+    ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
+    imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
+    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
+    wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
+    joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
+    controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));
+
+    // 初始化 ActionManager（在创建发布器和客户端之后）
+    auto is_jog_mode_func = [this]() -> bool { return isJogMode_; };
+    action_manager_ = std::make_unique<ActionManager>(
+        this,
+        robotControlManager_,
+        is_jog_mode_func,
+        motorCmdPub_,
+        motorParamClient_);
+    action_manager_->initialize();
+
+    lastTime_ = this->now();  // 初始化时间
+    std::cout << "RobotFsm Node is created finished!" << std::endl;
+}
+
+RobotControlNode::~RobotControlNode() 
+{
+    if (data_file_.is_open()) {
+        data_file_.close(); 
+    }
+}
+
+// ==================== 控制循环 ====================
+
+void RobotControlNode::ControlLoop() 
+{
+    // 计算时间步长
+    rclcpp::Time currentTime = this->now();
+    double dt_sec = (currentTime - lastTime_).seconds();
+    lastTime_ = currentTime;
+
+    // 处理停止请求
+    if (isStopping_)
+    {
+        action_manager_->set_move_home_executing(false);
+        action_manager_->set_move_leg_executing(false);
+        action_manager_->set_move_waist_executing(false);
+        action_manager_->set_move_wheel_executing(false);
+        isJogMode_ = false;
+
+        if (robotControlManager_.Stop(dt_sec))
+        {
+            isStopping_ = false;
+        }
+    }
+
+    if (action_manager_->is_move_home_executing())
+    {
+        if(robotControlManager_.GoHome(dt_sec))
+        {
+            robotControlManager_.SetJogWheel(false);
+            action_manager_->set_move_home_executing(false);
+            robotControlManager_.WheelReset();
+            robotControlManager_.ImuReset();
+        }
+    }
+
+    if (action_manager_->is_move_leg_executing())
+    {
+        if(robotControlManager_.MoveLeg(dt_sec))
+        {
+            action_manager_->set_move_leg_executing(false);
+        }
+    }
+
+    if (action_manager_->is_move_waist_executing())
+    {
+        if(robotControlManager_.MoveWaist(dt_sec))
+        {
+            action_manager_->set_move_waist_executing(false);
+        }
+    }
+
+    if (action_manager_->is_move_wheel_executing())
+    {
+        if(robotControlManager_.MoveWheel())
+        {
+            action_manager_->set_move_wheel_executing(false);
+        }
+    }
+
+    if (robotControlManager_.RobotInitFinished())
+    {
+        Publish_joint_trajectory();
+    }
+}
+
+void RobotControlNode::Publish_joint_trajectory() 
+{
+    std_msgs::msg::Float64MultiArray cmd_msg;
+    if (isJogMode_)
+    {
+        robotControlManager_.JogAxis(jogIndex_, jogDirection_);
+        cmd_msg = robotControlManager_.GetJointCommands();
+    }
+    else
+    {
+        cmd_msg = robotControlManager_.GetJointCommands();
+    }
+
+    #if RECORD_FLAG
+    data_file_ << 0;
+
+    // 2. 保存整个数组数据到txt文件
+    if (data_file_.is_open()) {
+        for (const auto& val : cmd_msg.data) {
+            data_file_ << "," << val;  // 用逗号分隔每个元素
+        }
+        
+        data_file_ << std::endl;  // 换行
+        data_file_.flush();  // 强制刷新
+    }
+
+    #endif
+
+    std_msgs::msg::String positionMsg;
+    std::stringstream msg_stream; 
+    size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
+
+    for (size_t i = 0; i < total_joints; ++i)
+    {
+        double current_pos = cmd_msg.data[i];
+
+        msg_stream << i << " pos " << current_pos;
+
+        if (i != total_joints - 1)
+        {
+            msg_stream << "; ";
+        }
+    }
+    positionMsg.data = msg_stream.str();
+
+    // std::cout << "publishing joint trajectory: " << positionMsg.data << std::endl;
+
+    ethercatSetPub_->publish(positionMsg);
+}
+
+void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
+    if (!msg) {  // 检查消息是否有效
+        RCLCPP_WARN(this->get_logger(), "收到空的joy消息,忽略");
+        return;
+    }
+
+    // 解析消息内容
+    std::string command = msg->data;
+    static std::string last_msg="";
+    if(last_msg!=command)
+    	last_msg=command;
+    else
+    	return;
+
+    if (command == "RB,1") {
+        if (isJogMode_ && jogDirection_ == 0)
+        {
+            if (jogIndex_ < robotControlManager_.GetMotionParameters().total_joints_ - 1)
+            {
+                jogIndex_ ++;
+                std::cout << "switch to jog axis: " << jogIndex_ + 1  << std::endl;
+            }
+            else
+            {
+                std::cout << "Reach the max joint, can't switch to next axis: " << jogIndex_ + 1  << std::endl;
+            }
+        }
+    }
+    else if (command == "LB,1") {
+        if (isJogMode_ && jogDirection_ == 0)
+        {
+            if (jogIndex_ > 0)
+            {
+                jogIndex_ --;
+                std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
+            }
+            else
+            {
+                std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ - 1 << std::endl;
+            }
+        }
+    }
+    else if (command == "A,1") {
+        if (!robotControlManager_.IsMoving(MovementPart::ALL))
+        {
+            isJogMode_ = 1;
+            std::cout << "jog mode on" << std::endl;
+        }
+        else
+        {
+            std::cout << "robot is moving, can't switch jog mode" << std::endl;
+        }
+    }
+    else if (command == "B,1") {
+        if (!robotControlManager_.IsMoving(MovementPart::ALL))
+        {
+            isJogMode_ = 0;
+            std::cout << "jog mode OFF" << std::endl;
+        }
+        else
+        {
+            std::cout << "robot is moving, can't switch jog mode" << std::endl;
+        }
+    }
+    else if (command == "方向垂直,-1.0") {
+        if (isJogMode_)
+        {
+            jogDirection_ = -1;
+            std::cout << "jog axis in negative direction: " << jogIndex_ + 1 << std::endl;
+        }
+    }
+    else if (command == "方向垂直,1.0") {
+        if (isJogMode_)
+        {
+            jogDirection_ = 1;
+            std::cout << "jog axis in positive direction: " << jogIndex_ + 1 << std::endl;
+        }
+    }
+    else if (command == "方向垂直,0.0") {
+        if (isJogMode_)
+        {
+            jogDirection_ = 0;
+            std::cout << "jog axis stopped: " << jogIndex_ + 1 << std::endl;
+        }
+    }
+    else if (command == "X,1") {
+        if (!isJogMode_)
+        {
+            isStopping_ = true;
+            std::cout << "stop robot" << std::endl;
+        }
+    }
+}
+
+void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) 
+{
+    if (!msg) {  // 检查消息是否有效
+        std::cout << "get null joint states!" << std::endl;
+        return;
+    }
+
+    robotControlManager_.UpdateJointStates(msg);
+}
+
+void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
+{
+    if (!cmd_msg) {  // 检查消息是否有效
+        std::cout << "get null imu msg!" << std::endl;
+        return;
+    }
+
+    robotControlManager_.UpdateImuMsg(cmd_msg);
+}
+
+void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
+{
+    if (!cmd_msg) {  // 检查消息是否有效
+        std::cout << "get null wheel states!" << std::endl;
+        return;
+    }
+    
+    robotControlManager_.UpdateWheelStates(cmd_msg);
+}
--- a/src/robot_control/src/main.cpp
+++ b/src/robot_control/src/main.cpp
@@ -1,7 +1,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <iostream>

-#include "robot_control_node.hpp"
+#include "core/robot_control_node.hpp"

 /**
 * @brief 程序入口函数
--- a/src/robot_control/src/robot_control_node.cpp
+++ b/src/robot_control/src/robot_control_node.cpp
@@ -1,752 +0,0 @@
-#include <thread>
-
-#include "rclcpp/rclcpp.hpp"
-#include "robot_control_node.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-using namespace robot_control;
-
-using namespace std;
-using namespace std_msgs::msg;
-
-namespace fs = std::filesystem;
-
-RobotControlNode::RobotControlNode() : Node("robot_control_node")
-{
-    RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
-    isJogMode_ = false;
-    jogDirection_ = 0;
-    jogIndex_ = 0;
-    lastSpeed_ = 51;
- 
-    // 初始化数据文件（设置路径，确保目录存在）
-    #if RECORD_FLAG
-    data_file_path_ = "/home/demo/ros2_joint_data.txt";  // 数据保存路径
-    fs::path file_path(data_file_path_);
-    fs::create_directories(file_path.parent_path());  // 自动创建父目录（如果不存在）
-    data_file_.open(data_file_path_, std::ios::trunc);
-    if (!data_file_.is_open()) {
-        
-    } else {
-        // 写入表头（仅在文件为空时）
-        data_file_.seekp(0, std::ios::end);  // 移动到文件末尾
-    }
-    #endif
-
-    using namespace std::placeholders;
-        
-    // 创建Action服务器
-    this->move_home_action_server_ = rclcpp_action::create_server<MoveHome>(
-        this,
-        "MoveHome",
-        std::bind(&RobotControlNode::handle_move_home_goal, this, _1, _2),
-        std::bind(&RobotControlNode::handle_move_home_cancel, this, _1),
-        std::bind(&RobotControlNode::handle_move_home_accepted, this, _1));
-    move_home_executing_ = false;
-        
-    RCLCPP_INFO(this->get_logger(), "MoveHome action server is ready");
-
-    this->move_leg_action_server_ = rclcpp_action::create_server<MoveLeg>(
-        this,
-        "MoveLeg",
-        std::bind(&RobotControlNode::handle_move_leg_goal, this, _1, _2),
-        std::bind(&RobotControlNode::handle_move_leg_cancel, this, _1),
-        std::bind(&RobotControlNode::handle_move_leg_accepted, this, _1));
-    move_leg_executing_ = false;
-
-    RCLCPP_INFO(this->get_logger(), "MoveLeg action server is ready");
-
-    this->move_waist_action_server_ = rclcpp_action::create_server<MoveWaist>(
-        this,
-        "MoveWaist",
-        std::bind(&RobotControlNode::handle_move_waist_goal, this, _1, _2),
-        std::bind(&RobotControlNode::handle_move_waist_cancel, this, _1),
-        std::bind(&RobotControlNode::handle_move_waist_accepted, this, _1));
-    move_waist_executing_ = false;
-
-    RCLCPP_INFO(this->get_logger(), "MoveWaist action server is ready");
-
-    this->move_wheel_action_server_ = rclcpp_action::create_server<MoveWheel>(
-        this,
-        "MoveWheel",
-        std::bind(&RobotControlNode::handle_move_wheel_goal, this, _1, _2),
-        std::bind(&RobotControlNode::handle_move_wheel_cancel, this, _1),
-        std::bind(&RobotControlNode::handle_move_wheel_accepted, this, _1));
-    move_wheel_executing_ = false;
-
-    RCLCPP_INFO(this->get_logger(), "MoveWheel action server is ready");
-
-    motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
-    ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
-    imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
-    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
-    wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
-    joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
-    controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));  // 绑定到新的控制函数);
-    motorParamClient_ = this->create_client<MotorParam>("/motor_param");
-
-    lastTime_ = this->now();  // 初始化时间
-    std::cout << "RobotFsm Node is created finished!" << std::endl;
-}
-
-RobotControlNode::~RobotControlNode() 
-{
-    if (data_file_.is_open()) {
-        data_file_.close(); 
-    }
-}
-
-rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
-    const rclcpp_action::GoalUUID & uuid,
-    std::shared_ptr<const MoveHome::Goal> goal)
-{
-    (void)goal;
-
-    if (robotControlManager_.IsMoving(MovementPart::ALL))
-    {
-         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (move_home_executing_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Another move home goal is executing");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (isJogMode_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-    
-    (void)uuid;
-    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-}
-    
-// 处理取消请求
-rclcpp_action::CancelResponse RobotControlNode::handle_move_home_cancel(
-    const std::shared_ptr<GoalHandleMoveHome> goal_handle)
-{
-    (void)goal_handle;
-    return rclcpp_action::CancelResponse::ACCEPT;
-}
-    
-// 接受目标后执行
-void RobotControlNode::handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
-{
-    move_home_executing_ = true;
-
-    using namespace std::placeholders;
-    std::thread{std::bind(&RobotControlNode::move_home_execute, this, _1), goal_handle}.detach();
-}
-
-// 执行目标计算
-void RobotControlNode::move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
-{
-    // RCLCPP_INFO(this->get_logger(), "Executing home goal");
-    rclcpp::Rate loop_rate(10); // 10Hz更新频率
-    const auto goal = goal_handle->get_goal();
-    auto feedback = std::make_shared<MoveHome::Feedback>();
-    auto result = std::make_shared<MoveHome::Result>();
-
-    while (move_home_executing_ && rclcpp::ok())
-    {
-        if (goal_handle->is_canceling())
-        {
-            result->success = false;
-            goal_handle->canceled(result);
-            RCLCPP_INFO(this->get_logger(), "Move home canceled");
-            move_home_executing_ = false;
-            //TODO: ADD STOP LOGIC
-            return;
-        }
-
-        auto joint_commands = robotControlManager_.GetJointFeedback(); 
-        feedback->joint_values.clear();
-
-        for (double val : joint_commands.data) {
-            feedback->joint_values.push_back(static_cast<int64_t>(val));
-        }
-        
-        goal_handle->publish_feedback(feedback);
-        
-        loop_rate.sleep();
-    }
-    
-    // 检查目标是否仍在活跃状态
-    if (rclcpp::ok())
-    {
-        result->success = true;
-        goal_handle->succeed(result);
-        RCLCPP_INFO(this->get_logger(), "move home succeeded.");
-    }
-}
-
-rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
-    const rclcpp_action::GoalUUID & uuid,
-    std::shared_ptr<const MoveLeg::Goal> goal)
-{
-    if (robotControlManager_.IsMoving(MovementPart::LEG))
-    {
-         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (move_leg_executing_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Another move leg goal is executing");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (isJogMode_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (!robotControlManager_.SetMoveLegParameters(goal->move_up_distance))
-    {
-        RCLCPP_ERROR(this->get_logger(), "Failed to set move leg parameters");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-    
-
-    (void)uuid;
-    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-}
-
-// 处理取消请求
-rclcpp_action::CancelResponse RobotControlNode::handle_move_leg_cancel(
-    const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
-{
-    (void)goal_handle;
-    return rclcpp_action::CancelResponse::ACCEPT;
-}
-
-// 接受目标后执行
-void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
-{
-    move_leg_executing_ = true;
-
-    using namespace std::placeholders;
-    std::thread{std::bind(&RobotControlNode::move_leg_execute, this, _1), goal_handle}.detach();
-}
-
-// 执行目标计算
-void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
-{
-    rclcpp::Rate loop_rate(10); // 10Hz更新频率
-    const auto goal = goal_handle->get_goal();
-    auto feedback = std::make_shared<MoveLeg::Feedback>();
-    auto result = std::make_shared<MoveLeg::Result>();
-
-    while (move_leg_executing_ && rclcpp::ok())
-    {
-        if (goal_handle->is_canceling())
-        {
-            result->success = false;
-            goal_handle->canceled(result);
-            RCLCPP_INFO(this->get_logger(), "move leg canceled");
-            move_leg_executing_ = false;
-            return;
-        }
-
-        auto joint_commands = robotControlManager_.GetJointFeedback(); 
-        feedback->joint_values.clear();
-
-        for (double val : joint_commands.data) {
-            feedback->joint_values.push_back(static_cast<int64_t>(val));
-        }
-        
-        goal_handle->publish_feedback(feedback);
-        
-        loop_rate.sleep();
-    }
-    
-    // 检查目标是否仍在活跃状态
-    if (rclcpp::ok())
-    {
-        result->success = true;
-        goal_handle->succeed(result);
-        RCLCPP_INFO(this->get_logger(), "move leg succeeded.");
-    }
-}
-
-rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
-    const rclcpp_action::GoalUUID & uuid,
-    std::shared_ptr<const MoveWaist::Goal> goal)
-{
-    if (robotControlManager_.IsMoving(MovementPart::WAIST))
-    {
-         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (move_waist_executing_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Another move waist goal is executing");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (isJogMode_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (!robotControlManager_.SetMoveWaistParameters(goal->move_pitch_degree, goal->move_yaw_degree))
-    {
-        RCLCPP_ERROR(this->get_logger(), "Invalid move waist request");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-    
-
-    (void)uuid;
-    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-}
-
-// 处理取消请求
-rclcpp_action::CancelResponse RobotControlNode::handle_move_waist_cancel(
-    const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
-{
-    (void)goal_handle;
-    return rclcpp_action::CancelResponse::ACCEPT;
-}
-
-// 接受目标后执行
-void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
-{
-    move_waist_executing_ = true;
-    using namespace std::placeholders;
-    std::thread{std::bind(&RobotControlNode::move_waist_execute, this, _1), goal_handle}.detach();
-}
-
-// 执行目标计算
-void RobotControlNode::move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
-{
-    // RCLCPP_INFO(this->get_logger(), "Executing waist goal");
-    rclcpp::Rate loop_rate(10); // 10Hz更新频率
-    const auto goal = goal_handle->get_goal(); // 获取目标
-    auto feedback = std::make_shared<MoveWaist::Feedback>();
-    auto result = std::make_shared<MoveWaist::Result>();
-
-    while (move_waist_executing_ && rclcpp::ok())
-    {
-        if (goal_handle->is_canceling())
-        {
-            result->success = false;
-            goal_handle->canceled(result);
-            RCLCPP_INFO(this->get_logger(), "move waist canceled");
-            move_waist_executing_ = false;
-            //TODO: ADD STOP LOGIC
-            return;
-        }
-
-        auto joint_commands = robotControlManager_.GetJointFeedback();
-        feedback->joint_values.clear();
-
-        for (double val : joint_commands.data) {
-            feedback->joint_values.push_back(static_cast<int64_t>(val));
-        }
-
-        goal_handle->publish_feedback(feedback);
-
-        loop_rate.sleep();
-
-    }
-    // 检查目标是否仍在活跃状态
-    if (rclcpp::ok())
-    {
-        result->success = true;
-        goal_handle->succeed(result);
-        RCLCPP_INFO(this->get_logger(), "move waist succeeded.");
-    }
-}
-
-rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
-    const rclcpp_action::GoalUUID & uuid,
-    std::shared_ptr<const MoveWheel::Goal> goal)
-{
-    if (robotControlManager_.IsMoving(MovementPart::WHEEL))
-    {
-         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (move_wheel_executing_)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Another move wheel goal is executing");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    if (goal->move_distance > 2.1 || goal->move_distance < -1.0 ||  goal->move_angle < -10 ||  goal->move_angle > 10 )
-    {
-        RCLCPP_ERROR(this->get_logger(), "exceed limit");
-        return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    (void)uuid;
-    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-}
-
-// 处理取消请求
-rclcpp_action::CancelResponse RobotControlNode::handle_move_wheel_cancel(
-    const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
-{
-    (void)goal_handle;
-    return rclcpp_action::CancelResponse::ACCEPT;
-}
-
-// 接受目标后执行
-void RobotControlNode::handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
-{
-    move_wheel_executing_ = true;
-    RCLCPP_INFO(this->get_logger(), "Goal accepted");
-    using namespace std::placeholders;
-    std::thread{std::bind(&RobotControlNode::move_wheel_execute, this, _1), goal_handle}.detach();
-}
-
-// 执行目标计算
-void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
-{
-    RCLCPP_INFO(this->get_logger(), "Executing wheel goal");
-    rclcpp::Rate loop_rate(10); // 10Hz更新频率
-    const auto goal = goal_handle->get_goal(); // 获取目标
-    auto feedback = std::make_shared<MoveWheel::Feedback>();
-    auto result = std::make_shared<MoveWheel::Result>();
-
-    double wheelAngle = 0;
-
-    if (abs(goal->move_angle) > 0)
-    {
-        robotControlManager_.SetJogWheel(true);
-        wheelAngle = goal->move_angle;
-    }
-
-    if (abs(goal->move_angle) == 0 && abs(goal->move_distance) == 0)
-    {
-        robotControlManager_.SetJogWheel(false);
-    }
-    
-    if (!robotControlManager_.GetJogWheel())
-    {
-        double tempValue = robotControlManager_.GetImuDifference()[2];
-        wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
-    }
-
-    robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
-
-    double ratio = robotControlManager_.GetWheelRatio();
-
-    auto request = std::make_shared<MotorParam::Request>();
-
-    if((goal->move_distance > 0.1 ) && !robotControlManager_.GetJogWheel())  // || goal->move_distance < -0.5
-    {
-        request->motor_id = 1;
-        request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
-        
-        request->add_acc = 8;
-        request->dec_acc = 8;
-        
-        motorParamClient_->async_send_request(request);
-
-        std::cout << "set motor 1 max speed : " << request->max_speed << std::endl;
-
-        rclcpp::sleep_for(std::chrono::milliseconds(50)); 
-    }
-    
-    robotControlManager_.MoveWheel();
-
-    std_msgs::msg::Float64MultiArray wheel_commands;
-    wheel_commands = robotControlManager_.GetWheelCommands();
-
-    MotorCmd wheel_commands_msg;
-    wheel_commands_msg.target = "rs485";
-    wheel_commands_msg.type = "bm";
-    wheel_commands_msg.position = "";
-    wheel_commands_msg.motor_id = {1,2};
-    wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]),(float)(wheel_commands.data[1])};
-
-    // std::cout << "wheel_commands.data[0] " << wheel_commands.data[0] << std::endl;
-    // std::cout << "wheel_commands.data[1] " << wheel_commands.data[1] << std::endl;
-
-    motorCmdPub_->publish(wheel_commands_msg);
-
-    while (move_wheel_executing_ && rclcpp::ok())
-    {
-        if (goal_handle->is_canceling())
-        {
-            result->success = false;
-            goal_handle->canceled(result);
-            RCLCPP_INFO(this->get_logger(), "move wheel canceled");
-            move_wheel_executing_ = false;
-            //TODO: ADD STOP LOGIC
-            return;
-        }
-
-        auto joint_feedback = robotControlManager_.GetWheelFeedback();
-
-        if (abs(joint_feedback.data[0] - wheel_commands.data[0]) < 20.0 && abs(joint_feedback.data[1] - wheel_commands.data[1]) < 20.0)
-        {
-            move_wheel_executing_ = false;
-        }
-
-        feedback->current_pos = joint_feedback.data[0];
-
-        //TODO: get the angle from lidar.
-        //feedback->current_angle = robotControlManager_.GetImuDifference()[2];
-
-        goal_handle->publish_feedback(feedback);
-
-        loop_rate.sleep();
-    }
-
-    if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel())
-    {
-        request->motor_id = 1;
-        request->max_speed = 51;
-        request->add_acc = 8;
-        request->dec_acc = 8;
-        
-        motorParamClient_->async_send_request(request);
-    }
-
-    // 检查目标是否仍在活跃状态
-    if (rclcpp::ok())
-    {
-        result->success = true;
-        goal_handle->succeed(result);
-        RCLCPP_INFO(this->get_logger(), "move wheel succeeded.");
-    }
-}
-
-// 状态机主循环
-void RobotControlNode::ControlLoop() {
-    // 计算时间差
-    rclcpp::Time current_time = this->now();
-    rclcpp::Duration dt = current_time - lastTime_;
-    double dt_sec = dt.seconds();
-    lastTime_ = current_time;
-
-    if (isStopping_)
-    {
-        move_home_executing_ = false;
-        move_leg_executing_ = false;
-        move_waist_executing_ = false;
-        isJogMode_ = false;
-
-        if (robotControlManager_.Stop(dt_sec))
-        {
-            isStopping_ = false;
-        }
-    }
-
-    if (move_home_executing_)
-    {
-        if(robotControlManager_.GoHome(dt_sec))
-        {
-            robotControlManager_.SetJogWheel(false);
-            move_home_executing_ = false;
-            robotControlManager_.WheelReset();
-            robotControlManager_.ImuReset();
-        }
-    }
-
-    if (move_leg_executing_)
-    {
-        if(robotControlManager_.MoveLeg(dt_sec))
-        {
-            move_leg_executing_ = false;
-        }
-    }
-
-    if (move_waist_executing_)
-    {
-        if(robotControlManager_.MoveWaist(dt_sec))
-        {
-            move_waist_executing_ = false;
-        }
-    }
-
-    if (robotControlManager_.RobotInitFinished())
-    {
-        Publish_joint_trajectory();
-    }
-}
-
-
-void RobotControlNode::Publish_joint_trajectory() 
-{
-    std_msgs::msg::Float64MultiArray cmd_msg;
-    if (isJogMode_)
-    {
-        robotControlManager_.JogAxis(jogIndex_, jogDirection_);
-        cmd_msg = robotControlManager_.GetJointCommands();
-    }
-    else
-    {
-        cmd_msg = robotControlManager_.GetJointCommands();
-    }
-
-    #if RECORD_FLAG
-    data_file_ << 0;
-
-    // 2. 保存整个数组数据到txt文件
-    if (data_file_.is_open()) {
-        for (const auto& val : cmd_msg.data) {
-            data_file_ << "," << val;  // 用逗号分隔每个元素
-        }
-        
-        data_file_ << std::endl;  // 换行
-        data_file_.flush();  // 强制刷新
-    }
-
-    #endif
-
-    std_msgs::msg::String positionMsg;
-    std::stringstream msg_stream; 
-    size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
-
-    for (size_t i = 0; i < total_joints; ++i)
-    {
-        double current_pos = cmd_msg.data[i];
-
-        msg_stream << i << " pos " << current_pos;
-
-        if (i != total_joints - 1)
-        {
-            msg_stream << "; ";
-        }
-    }
-    positionMsg.data = msg_stream.str();
-
-    // std::cout << "publishing joint trajectory: " << positionMsg.data << std::endl;
-
-    ethercatSetPub_->publish(positionMsg);
-}
-
-void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
-    if (!msg) {  // 检查消息是否有效
-        RCLCPP_WARN(this->get_logger(), "收到空的joy消息,忽略");
-        return;
-    }
-
-    // 解析消息内容
-    std::string command = msg->data;
-    static std::string last_msg="";
-    if(last_msg!=command)
-    	last_msg=command;
-    else
-    	return;
-
-    if (command == "RB,1") {
-        if (isJogMode_ && jogDirection_ == 0)
-        {
-            if (jogIndex_ < robotControlManager_.GetMotionParameters().total_joints_ - 1)
-            {
-                jogIndex_ ++;
-                std::cout << "switch to jog axis: " << jogIndex_ + 1  << std::endl;
-            }
-            else
-            {
-                std::cout << "Reach the max joint, can't switch to next axis: " << jogIndex_ + 1  << std::endl;
-            }
-        }
-    }
-    else if (command == "LB,1") {
-        if (isJogMode_ && jogDirection_ == 0)
-        {
-            if (jogIndex_ > 0)
-            {
-                jogIndex_ --;
-                std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
-            }
-            else
-            {
-                std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ - 1 << std::endl;
-            }
-        }
-    }
-    else if (command == "A,1") {
-        if (!robotControlManager_.IsMoving(MovementPart::ALL))
-        {
-            isJogMode_ = 1;
-            std::cout << "jog mode on" << std::endl;
-        }
-        else
-        {
-            std::cout << "robot is moving, can't switch jog mode" << std::endl;
-        }
-    }
-    else if (command == "B,1") {
-        if (!robotControlManager_.IsMoving(MovementPart::ALL))
-        {
-            isJogMode_ = 0;
-            std::cout << "jog mode OFF" << std::endl;
-        }
-        else
-        {
-            std::cout << "robot is moving, can't switch jog mode" << std::endl;
-        }
-    }
-    else if (command == "方向垂直,-1.0") {
-        if (isJogMode_)
-        {
-            jogDirection_ = -1;
-            std::cout << "jog axis in negative direction: " << jogIndex_ + 1 << std::endl;
-        }
-    }
-    else if (command == "方向垂直,1.0") {
-        if (isJogMode_)
-        {
-            jogDirection_ = 1;
-            std::cout << "jog axis in positive direction: " << jogIndex_ + 1 << std::endl;
-        }
-    }
-    else if (command == "方向垂直,0.0") {
-        if (isJogMode_)
-        {
-            jogDirection_ = 0;
-            std::cout << "jog axis stopped: " << jogIndex_ + 1 << std::endl;
-        }
-    }
-    else if (command == "X,1") {
-        if (!isJogMode_)
-        {
-            isStopping_ = true;
-            std::cout << "stop robot" << std::endl;
-        }
-    }
-}
-
-void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) 
-{
-    if (!msg) {  // 检查消息是否有效
-        std::cout << "get null joint states!" << std::endl;
-        return;
-    }
-
-    robotControlManager_.UpdateJointStates(msg);
-}
-
-void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
-{
-    if (!cmd_msg) {  // 检查消息是否有效
-        std::cout << "get null imu msg!" << std::endl;
-        return;
-    }
-
-    robotControlManager_.UpdateImuMsg(cmd_msg);
-}
-
-void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
-{
-    if (!cmd_msg) {  // 检查消息是否有效
-        std::cout << "get null wheel states!" << std::endl;
-        return;
-    }
-    
-    robotControlManager_.UpdateWheelStates(cmd_msg);
-}
-
--- a/src/robot_control/src/utils/extended_kalman_filter.cpp
+++ b/src/robot_control/src/utils/extended_kalman_filter.cpp
@@ -1,4 +1,4 @@
-#include "extended_kalman_filter.hpp"
+#include "utils/extended_kalman_filter.hpp"

 namespace robot_control {
 EKF::EKF(double init_x, double init_y, double init_theta, double init_omega) 
--- a/src/robot_control/src/utils/robot_kinematics.cpp
+++ b/src/robot_control/src/utils/robot_kinematics.cpp
@@ -1,4 +1,4 @@
-#include "robot_kinematics.hpp"
+#include "utils/robot_kinematics.hpp"
 #include <cmath>
 #include <stdexcept>
 #include <iostream>
--- a/src/robot_control/src/utils/robot_model.cpp
+++ b/src/robot_control/src/utils/robot_model.cpp
@@ -1,4 +1,4 @@
-#include "robot_model.hpp"
+#include "utils/robot_model.hpp"
 #include <iostream>
 #include <cmath>

--- a/src/robot_control/src/utils/trapezoidal_trajectory.cpp
+++ b/src/robot_control/src/utils/trapezoidal_trajectory.cpp
@@ -1,4 +1,4 @@
-#include "trapezoidal_trajectory.hpp"
+#include "utils/trapezoidal_trajectory.hpp"
 #include <cmath>
 #include <stdexcept>
 #include <algorithm>
--- a/src/robot_control/src/utils/urdf_parser.cpp
+++ b/src/robot_control/src/utils/urdf_parser.cpp
@@ -1,4 +1,4 @@
-#include "urdf_parser.hpp"
+#include "utils/urdf_parser.hpp"
 #include <fstream>
 #include <sstream>
 #include <algorithm>