draft version

CMakeLists.txt

@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 3.10)
+project(robot_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# 寻找依赖包
+find_package(Boost REQUIRED COMPONENTS python3)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+# 生成Action接口
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/RobotMotion.action"
+  DEPENDENCIES geometry_msgs
+)
+
+# 包含目录
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIRS}
+  ${pinocchio_INCLUDE_DIRS}
+  ${Python3_INCLUDE_DIRS}  # 添加 Python 头文件目录（消除 Boost 警告）
+)
+
+# 链接库
+link_directories(
+  ${pinocchio_LIBRARY_DIRS}
+  ${Boost_LIBRARIES}        # 链接 Boost 库（消除警告）
+  ${Python3_LIBRARIES}      # 链接 Python 库（消除警告）
+)
+
+# 生成库
+add_library(robot_control_lib
+  src/robot_control/model/RobotModel.cpp
+  src/robot_control/model/UpperBodyModel.cpp
+  src/robot_control/model/LowerBodyModel.cpp
+  src/robot_control/control/ControllerBase.cpp
+  src/robot_control/control/UpperBodyController.cpp
+  src/robot_control/control/LowerBodyController.cpp
+  src/robot_control/ros/RobotMotionAction.cpp
+  src/robot_control/planning/RealtimeInterpolator.cpp
+  src/robot_control/state_machine/RobotStateMachine.cpp
+  src/robot_control/RobotControlSystem.cpp
+)
+
+# 链接依赖
+ament_target_dependencies(robot_control_lib
+  rclcpp
+  rclcpp_action
+  geometry_msgs
+)
+
+target_link_libraries(robot_control_lib
+  pinocchio::pinocchio
+)
+
+# 生成可执行文件
+add_executable(robot_control_node
+  src/main.cpp
+)
+
+# 链接可执行文件
+target_link_libraries(robot_control_node
+  robot_control_lib
+)
+
+# 安装目标
+install(TARGETS
+  robot_control_lib
+  robot_control_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# 安装头文件
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+# 安装URDF等资源文件
+install(DIRECTORY urdf/
+  DESTINATION share/${PROJECT_NAME}/urdf/
+)
+
+# 注册依赖
+ament_export_include_directories(include)
+ament_export_libraries(robot_control_lib)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(rclcpp_action)
+ament_export_dependencies(pinocchio)
+
+rosidl_get_typesupport_target(cpp_typesupport_target
+  ${PROJECT_NAME} rosidl_typesupport_cpp)
+
+target_link_libraries(robot_control_lib "${cpp_typesupport_target}")
+
+ament_package()

README.md

@@ -1,2 +1,16 @@
 # hivecore_robot_motion
 
+## 简介
+
+本项目为HiveCore机器人运动控制模块，包含运动控制算法、运动控制接口、运动控制测试等。
+
+## 功能
+
+- 运动控制算法：包括位置控制、速度控制、加速度控制等。
+- 运动控制接口：包括运动控制命令、运动控制状态查询等。
+- 运动控制测试：包括运动控制命令发送、运动控制状态查询等。
+
+## 使用方法
+
+1. 安装依赖库：在项目根目录下执行`pip install -r requirements.txt`命令安装依赖库。
+

action/RobotMotion.action

@@ -0,0 +1,21 @@
+# 机器人运动目标
+float64 x                # x位置 (米)，用于移动基座
+float64 y                # y位置 (米)，用于移动基座
+float64 yaw              # 偏航角 (弧度)，用于移动基座
+int32 arm_index          # 手臂索引 (0:左臂, 1:右臂, -1:不使用)
+float64[] arm_position   # 手臂位置目标 (x, y, z)，米
+int32 leg_index          # 腿部索引 (0-3, -1:不使用)
+float64[] leg_position   # 腿部位置目标 (x, y, z)，米
+float64[] body_pose      # 身体姿态 (roll, pitch, yaw, height)，弧度/米
+float64 duration         # 期望运动时间 (秒)，0表示使用默认
+bool blocking            # 是否阻塞等待完成
+---
+# 运动结果
+bool success             # 是否成功
+string message           # 结果消息
+float64 actual_duration  # 实际运动时间 (秒)
+---
+# 运动反馈
+float64 progress         # 进度 (0.0-1.0)
+float64[] current_state  # 当前状态
+string status            # 状态描述

include/robot_control/RobotControlSystem.h

@@ -0,0 +1,93 @@
+#ifndef ROBOT_CONTROL_SYSTEM_H
+#define ROBOT_CONTROL_SYSTEM_H
+
+#include "robot_control/model/UpperBodyModel.h"
+#include "robot_control/model/LowerBodyModel.h"
+#include "robot_control/control/UpperBodyController.h"
+#include "robot_control/control/LowerBodyController.h"
+#include "robot_control/ros/RobotMotionAction.h"
+#include "robot_control/planning/RealtimeInterpolator.h"
+#include "robot_control/state_machine/RobotStateMachine.h"
+#include <rclcpp/rclcpp.hpp>
+#include <ocs2_core/Types.h>
+
+namespace robot_control {
+
+class RobotControlSystem {
+public:
+    using Ptr = std::shared_ptr<RobotControlSystem>;
+    using ConstPtr = std::shared_ptr<const RobotControlSystem>;
+
+    RobotControlSystem(rclcpp::Node::SharedPtr node);
+    ~RobotControlSystem() = default;
+    
+    // 初始化系统
+    bool initialize(const std::string& upperBodyUrdf, const std::string& lowerBodyUrdf);
+    
+    // 启动系统
+    void start();
+    
+    // 停止系统
+    void stop();
+    
+    // 更新系统状态
+    void update();
+    
+    // 获取当前状态
+    state_machine::RobotState getCurrentState() const;
+    
+    // 处理运动目标
+    void handleMotionGoal(const ros::MotionGoal& goal);
+    
+    // 处理运动取消
+    void handleMotionCancel();
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    
+    // 模型
+    model::UpperBodyModel::Ptr upperBodyModel_;
+    model::LowerBodyModel::Ptr lowerBodyModel_;
+    
+    // 控制器
+    control::UpperBodyController::Ptr upperBodyController_;
+    control::LowerBodyController::Ptr lowerBodyController_;
+    
+    // ROS通信
+    ros::RobotMotionAction::Ptr motionAction_;
+    
+    // 实时插补器
+    planning::RealtimeInterpolator::Ptr upperBodyInterpolator_;
+    planning::RealtimeInterpolator::Ptr lowerBodyInterpolator_;
+    
+    // 状态机
+    state_machine::RobotStateMachine::Ptr stateMachine_;
+    
+    // 系统状态
+    ocs2::vector_t upperBodyState_;
+    ocs2::vector_t lowerBodyState_;
+    ocs2::vector_t upperBodyCommand_;
+    ocs2::vector_t lowerBodyCommand_;
+    
+    // 控制频率
+    double controlFrequency_;
+    
+    // 定时器
+    rclcpp::TimerBase::SharedPtr controlTimer_;
+    
+    // 状态机回调函数
+    void registerStateCallbacks();
+    
+    // 控制回调
+    void controlCallback();
+    
+    // 发布反馈
+    void publishFeedback();
+    
+    // 处理状态转换
+    void onStateTransition(state_machine::RobotState newState);
+};
+
+} // namespace robot_control
+
+#endif // ROBOT_CONTROL_SYSTEM_H

include/robot_control/control/ArmController.h

@@ -0,0 +1,41 @@
+#ifndef UPPER_BODY_CONTROLLER_H
+#define UPPER_BODY_CONTROLLER_H
+
+#include "robot_control/control/ControllerBase.h"
+
+namespace robot_control {
+namespace control {
+
+class UpperBodyController : public ControllerBase {
+public:
+    using Ptr = std::shared_ptr<UpperBodyController>;
+    using ConstPtr = std::shared_ptr<const UpperBodyController>;
+
+    UpperBodyController(model::UpperBodyModel::Ptr model);
+    ~UpperBodyController() override = default;
+
+    // 从基类继承的方法
+    ocs2::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) override;
+    void reset() override;
+    bool isGoalReached(const ocs2::vector_t& x) const override;
+    
+    // 设置目标轨迹
+    void setTargetTrajectories(const ocs2::TargetTrajectories& targetTrajectories);
+    
+    // 设置单个手臂目标位置
+    void setArmTargetPosition(bool leftArm, const Eigen::Vector3d& targetPos);
+
+private:
+    model::UpperBodyModel::Ptr model_;
+    std::unique_ptr<ocs2::Ocs2Solver> solver_;
+    ocs2::TargetTrajectories targetTrajectories_;
+    
+    // 控制器参数
+    double positionTolerance_ = 0.01; // 位置容差 (m)
+    double velocityTolerance_ = 0.05; // 速度容差 (m/s)
+};
+
+} // namespace control
+} // namespace robot_control
+
+#endif // UPPER_BODY_CONTROLLER_H

include/robot_control/control/ControllerBase.h

@@ -0,0 +1,31 @@
+#ifndef CONTROLLER_BASE_H
+#define CONTROLLER_BASE_H
+
+#include <memory>
+#include <Eigen/Dense>
+
+namespace robot_control {
+namespace control {
+
+class ControllerBase {
+public:
+    using Ptr = std::shared_ptr<ControllerBase>;
+    using ConstPtr = std::shared_ptr<const ControllerBase>;
+
+    ControllerBase() = default;
+    ~ControllerBase() override = default;
+
+    // 计算控制输入
+    virtual EIGEN::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) = 0;
+    
+    // 重置控制器
+    virtual void reset() = 0;
+    
+    // 检查是否到达目标
+    virtual bool isGoalReached(const ocs2::vector_t& x) const = 0;
+};
+
+} // namespace control
+} // namespace robot_control
+
+#endif // CONTROLLER_BASE_H

include/robot_control/control/LegController.h

@@ -0,0 +1,48 @@
+#ifndef LOWER_BODY_CONTROLLER_H
+#define LOWER_BODY_CONTROLLER_H
+
+#include "robot_control/control/ControllerBase.h"
+#include "robot_control/model/LowerBodyModel.h"
+#include <ocs2_oc/oc_solver/Ocs2Solver.h>
+#include <ocs2_core/reference/TargetTrajectories.h>
+
+namespace robot_control {
+namespace control {
+
+class LowerBodyController : public ControllerBase {
+public:
+    using Ptr = std::shared_ptr<LowerBodyController>;
+    using ConstPtr = std::shared_ptr<const LowerBodyController>;
+
+    LowerBodyController(model::LowerBodyModel::Ptr model);
+    ~LowerBodyController() override = default;
+
+    // 从基类继承的方法
+    ocs2::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) override;
+    void reset() override;
+    bool isGoalReached(const ocs2::vector_t& x) const override;
+    
+    // 设置目标轨迹
+    void setTargetTrajectories(const ocs2::TargetTrajectories& targetTrajectories);
+    
+    // 设置轮子速度
+    void setWheelVelocities(const Eigen::Vector4d& wheelVels);
+    
+    // 设置单腿目标位置
+    void setLegTargetPosition(int legIndex, const Eigen::Vector3d& targetPos);
+
+private:
+    model::LowerBodyModel::Ptr model_;
+    std::unique_ptr<ocs2::Ocs2Solver> solver_;
+    ocs2::TargetTrajectories targetTrajectories_;
+    Eigen::Vector4d wheelVelocities_;
+    
+    // 控制器参数
+    double positionTolerance_ = 0.01; // 位置容差 (m)
+    double velocityTolerance_ = 0.05; // 速度容差 (m/s)
+};
+
+} // namespace control
+} // namespace robot_control
+
+#endif // LOWER_BODY_CONTROLLER_H

include/robot_control/model/RobotModel.h

@@ -0,0 +1,35 @@
+#ifndef ROBOT_MODEL_H
+#define ROBOT_MODEL_H
+
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+#include <string>
+#include <memory>
+
+namespace robot_control {
+namespace model {
+
+class RobotModel {
+public:
+    using Ptr = std::shared_ptr<RobotModel>;
+    using ConstPtr = std::shared_ptr<const RobotModel>;
+
+    RobotModel(const std::string& urdfPath);
+    virtual ~RobotModel() = default;
+
+    const pinocchio::Model& getModel() const { return model_; }
+    pinocchio::Data& getData() { return data_; }
+    const pinocchio::Data& getData() const { return data_; }
+
+    virtual void update(const Eigen::VectorXd& q, const Eigen::VectorXd& v) const;
+
+protected:
+    pinocchio::Model model_;
+    mutable pinocchio::Data data_;
+};
+
+} // namespace model
+} // namespace robot_control
+
+#endif // ROBOT_MODEL_H

include/robot_control/planning/RealtimeInterpolation.h

@@ -0,0 +1,86 @@
+#ifndef REALTIME_INTERPOLATOR_H
+#define REALTIME_INTERPOLATOR_H
+
+#include <ocs2_core/reference/TargetTrajectories.h>
+#include <Eigen/Dense>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <atomic>
+#include <functional>
+
+namespace robot_control {
+namespace planning {
+
+class RealtimeInterpolator {
+public:
+    using Ptr = std::shared_ptr<RealtimeInterpolator>;
+    using ConstPtr = std::shared_ptr<const RealtimeInterpolator>;
+    
+    // 插值类型
+    enum class InterpolationType {
+        LINEAR,    // 线性插值
+        CUBIC,     // 三次多项式插值
+        QUINTIC    // 五次多项式插值
+    };
+    
+    RealtimeInterpolator(double controlFrequency = 1000.0);
+    ~RealtimeInterpolator();
+    
+    // 设置目标轨迹
+    void setTargetTrajectories(const ocs2::TargetTrajectories& trajectories, 
+                              InterpolationType type = InterpolationType::CUBIC);
+    
+    // 获取当前参考点
+    bool getCurrentReference(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 停止插值线程
+    void stop();
+    
+    // 注册控制输出回调
+    using ControlCallback = std::function<void(double, const ocs2::vector_t&, const ocs2::vector_t&)>;
+    void setControlCallback(ControlCallback callback);
+    
+    // 检查是否正在执行轨迹
+    bool isActive() const { return isActive_; }
+    
+    // 检查是否到达目标
+    bool isGoalReached() const { return isGoalReached_; }
+
+private:
+    // 插值线程主函数
+    void interpolationThread();
+    
+    // 根据类型进行插值计算
+    void interpolate(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 线性插值
+    void linearInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 三次多项式插值
+    void cubicInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 五次多项式插值
+    void quinticInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    ocs2::TargetTrajectories trajectories_;
+    InterpolationType interpolationType_;
+    
+    double controlFrequency_;
+    double dt_;
+    
+    std::thread interpolationThread_;
+    std::mutex mutex_;
+    std::condition_variable cv_;
+    
+    std::atomic<bool> running_;
+    std::atomic<bool> isActive_;
+    std::atomic<bool> isGoalReached_;
+    
+    ControlCallback controlCallback_;
+};
+
+} // namespace planning
+} // namespace robot_control
+
+#endif // REALTIME_INTERPOLATOR_H

include/robot_control/ros/RobotMotionAction.h

@@ -0,0 +1,131 @@
+#ifndef ROBOT_MOTION_ACTION_H
+#define ROBOT_MOTION_ACTION_H
+
+#include <action_msgs/msg/goal_status.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <Eigen/Dense>
+#include <string>
+
+// 定义Action消息类型
+namespace robot_control {
+namespace ros {
+
+// 运动目标
+struct MotionGoal {
+    // 运动类型：移动、操作、姿态调整等
+    enum class Type {
+        MOVE_BASE,        // 移动基座
+        ARM_MOTION,       // 手臂运动
+        LEG_MOTION,       // 腿部运动
+        BODY_POSE,        // 身体姿态
+        WHOLE_BODY_MOTION // 全身运动
+    };
+
+    Type type;
+    
+    // 基础移动目标 (用于MOVE_BASE类型)
+    struct BaseTarget {
+        double x;     // x位置 (m)
+        double y;     // y位置 (m)
+        double yaw;   // 偏航角 (rad)
+        double speed; // 移动速度 (m/s)
+    };
+    
+    // 手臂运动目标 (用于ARM_MOTION类型)
+    struct ArmTarget {
+        bool leftArm;          // 是否是左臂
+        Eigen::Vector3d position; // 位置目标 (m)
+        Eigen::Vector3d orientation; // 姿态目标 (rad)
+        bool relative;         // 是否是相对运动
+    };
+    
+    // 腿部运动目标 (用于LEG_MOTION类型)
+    struct LegTarget {
+        int legIndex;          // 腿索引 (0-3)
+        Eigen::Vector3d position; // 位置目标 (m)
+        bool relative;         // 是否是相对运动
+    };
+    
+    // 身体姿态目标 (用于BODY_POSE类型)
+    struct BodyPoseTarget {
+        double roll;  // 横滚角 (rad)
+        double pitch; // 俯仰角 (rad)
+        double yaw;   // 偏航角 (rad)
+        double height; // 高度 (m)
+    };
+
+    // 根据运动类型选择对应的目标数据
+    union {
+        BaseTarget base;
+        ArmTarget arm;
+        LegTarget leg;
+        BodyPoseTarget bodyPose;
+    } target;
+    
+    // 运动参数
+    double duration; // 期望运动时间 (s)，0表示使用默认
+    bool blocking;   // 是否阻塞等待完成
+};
+
+// 运动反馈
+struct MotionFeedback {
+    double progress; // 进度 (0.0-1.0)
+    Eigen::VectorXd currentState; // 当前状态
+    std::string status; // 状态描述
+};
+
+// 运动结果
+struct MotionResult {
+    bool success;    // 是否成功
+    std::string message; // 结果消息
+    double actualDuration; // 实际运动时间 (s)
+};
+
+// Action接口类
+class RobotMotionAction {
+public:
+    using Ptr = std::shared_ptr<RobotMotionAction>;
+    
+    using GoalHandle = rclcpp_action::ServerGoalHandle<robot_control_msgs::action::RobotMotion>;
+    
+    RobotMotionAction(rclcpp::Node::SharedPtr node);
+    
+    // 发送目标
+    void sendGoal(const MotionGoal& goal);
+    
+    // 取消目标
+    void cancelGoal();
+    
+    // 注册目标回调
+    using GoalCallback = std::function<void(const MotionGoal&)>;
+    void setGoalCallback(GoalCallback callback);
+    
+    // 注册取消回调
+    using CancelCallback = std::function<void()>;
+    void setCancelCallback(CancelCallback callback);
+    
+    // 发布反馈
+    void publishFeedback(const MotionFeedback& feedback);
+    
+    // 发布结果
+    void publishResult(GoalHandle::SharedPtr goalHandle, const MotionResult& result);
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    rclcpp_action::Server<robot_control_msgs::action::RobotMotion>::SharedPtr actionServer_;
+    
+    GoalCallback goalCallback_;
+    CancelCallback cancelCallback_;
+    
+    // Action回调函数
+    rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, 
+                                          std::shared_ptr<const robot_control_msgs::action::RobotMotion::Goal> goal);
+    rclcpp_action::CancelResponse handleCancel(const std::shared_ptr<GoalHandle> goalHandle);
+    void handleAccepted(const std::shared_ptr<GoalHandle> goalHandle);
+};
+
+} // namespace ros
+} // namespace robot_control
+
+#endif // ROBOT_MOTION_ACTION_H

include/robot_control/state_machine/RobotStateMachine.h

@@ -0,0 +1,93 @@
+#ifndef ROBOT_STATE_MACHINE_H
+#define ROBOT_STATE_MACHINE_H
+
+#include <ocs2_core/Types.h>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <functional>
+
+namespace robot_control {
+namespace state_machine {
+
+// 机器人状态定义
+enum class RobotState {
+    IDLE,           // 空闲
+    MOVING_BASE,    // 基座移动
+    ARM_MOTION,     // 手臂运动
+    LEG_MOTION,     // 腿部运动
+    BODY_ADJUST,    // 身体调整
+    TRANSITION,     // 状态过渡
+    ERROR           // 错误状态
+};
+
+// 事件定义
+enum class Event {
+    START_MOVE_BASE,
+    START_ARM_MOTION,
+    START_LEG_MOTION,
+    START_BODY_ADJUST,
+    MOTION_COMPLETED,
+    MOTION_FAILED,
+    STOP_MOTION,
+    ERROR_OCCURRED,
+    RECOVER_FROM_ERROR
+};
+
+// 状态转换函数
+using TransitionFunction = std::function<RobotState(const RobotState&, const Event&)>;
+
+// 状态进入/退出回调
+using StateCallback = std::function<void()>;
+
+class RobotStateMachine {
+public:
+    using Ptr = std::shared_ptr<RobotStateMachine>;
+    using ConstPtr = std::shared_ptr<const RobotStateMachine>;
+
+    RobotStateMachine();
+    
+    // 获取当前状态
+    RobotState getCurrentState() const { return currentState_; }
+    
+    // 处理事件
+    void handleEvent(Event event);
+    
+    // 注册状态转换规则
+    void registerTransition(RobotState fromState, Event event, RobotState toState);
+    
+    // 注册状态进入回调
+    void registerEnterCallback(RobotState state, StateCallback callback);
+    
+    // 注册状态退出回调
+    void registerExitCallback(RobotState state, StateCallback callback);
+    
+    // 获取状态的字符串表示
+    std::string stateToString(RobotState state) const;
+    
+    // 获取事件的字符串表示
+    std::string eventToString(Event event) const;
+
+private:
+    RobotState currentState_;
+    
+    // 状态转换表: fromState -> event -> toState
+    std::unordered_map<RobotState, std::unordered_map<Event, RobotState>> transitions_;
+    
+    // 状态进入回调
+    std::unordered_map<RobotState, StateCallback> enterCallbacks_;
+    
+    // 状态退出回调
+    std::unordered_map<RobotState, StateCallback> exitCallbacks_;
+    
+    // 检查是否允许转换
+    bool canTransition(RobotState fromState, Event event) const;
+    
+    // 执行状态转换
+    void transitionTo(RobotState newState);
+};
+
+} // namespace state_machine
+} // namespace robot_control
+
+#endif // ROBOT_STATE_MACHINE_H

package.xml

@@ -0,0 +1,27 @@
+<?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>0.0.0</version>
+  <description>轮足双臂机器人运动控制系统</description>
+  <maintainer email="ray@hivecore.cn">Ray</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_action</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>pinocchio</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>pinocchio</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

setup.sh

@@ -0,0 +1,3 @@
+sudo apt-get install ros-humble-pinocchio
+sudo apt-get install libboost-python-dev
+sudo apt-get install libboost-python3-dev

src/main.cpp

@@ -0,0 +1,40 @@
+#include "robot_control/RobotControlSystem.h"
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+
+int main(int argc, char* argv[]) {
+    // 初始化ROS
+    rclcpp::init(argc, argv);
+    
+    // 创建节点
+    auto node = rclcpp::Node::make_shared("robot_control_node");
+    
+    // 声明URDF路径参数
+    node->declare_parameter<std::string>("upper_body_urdf", "urdf/upper_body.urdf");
+    node->declare_parameter<std::string>("lower_body_urdf", "urdf/lower_body.urdf");
+    
+    // 获取URDF路径
+    std::string upperBodyUrdf, lowerBodyUrdf;
+    node->get_parameter("upper_body_urdf", upperBodyUrdf);
+    node->get_parameter("lower_body_urdf", lowerBodyUrdf);
+    
+    // 创建并初始化控制系统
+    auto controlSystem = std::make_shared<robot_control::RobotControlSystem>(node);
+    if (!controlSystem->initialize(upperBodyUrdf, lowerBodyUrdf)) {
+        RCLCPP_ERROR(node->get_logger(), "Failed to initialize control system");
+        return 1;
+    }
+    
+    // 启动控制系统
+    controlSystem->start();
+    
+    // 运行ROS循环
+    rclcpp::spin(node);
+    
+    // 停止系统
+    controlSystem->stop();
+    
+    // 关闭ROS
+    rclcpp::shutdown();
+    return 0;
+}

src/robot_control/RobotControlSystem.cpp

@@ -0,0 +1,222 @@
+#include "robot_control/RobotControlSystem.h"
+#include "robot_control/state_machine/RobotStateMachine.h"
+#include <chrono>
+#include <thread>
+
+namespace robot_control {
+
+RobotControlSystem::RobotControlSystem(rclcpp::Node::SharedPtr node) 
+    : node_(node),
+      controlFrequency_(1000.0) {
+    // 从参数服务器获取控制频率
+    node_->get_parameter_or("control_frequency", controlFrequency_, 1000.0);
+    
+    // 初始化组件
+    motionAction_ = std::make_shared<ros::RobotMotionAction>(node_);
+    stateMachine_ = std::make_shared<state_machine::RobotStateMachine>();
+    
+    // 创建插值器
+    upperBodyInterpolator_ = std::make_shared<planning::RealtimeInterpolator>(controlFrequency_);
+    lowerBodyInterpolator_ = std::make_shared<planning::RealtimeInterpolator>(controlFrequency_);
+    
+    // 设置Action回调
+    motionAction_->setGoalCallback(
+        std::bind(&RobotControlSystem::handleMotionGoal, this, std::placeholders::_1)
+    );
+    
+    motionAction_->setCancelCallback(
+        std::bind(&RobotControlSystem::handleMotionCancel, this)
+    );
+    
+    // 注册状态回调
+    registerStateCallbacks();
+}
+
+bool RobotControlSystem::initialize(const std::string& upperBodyUrdf, const std::string& lowerBodyUrdf) {
+    try {
+        // 加载机器人模型
+        upperBodyModel_ = std::make_shared<model::UpperBodyModel>(upperBodyUrdf);
+        lowerBodyModel_ = std::make_shared<model::LowerBodyModel>(lowerBodyUrdf);
+        
+        // 初始化控制器
+        upperBodyController_ = std::make_shared<control::UpperBodyController>(upperBodyModel_);
+        lowerBodyController_ = std::make_shared<control::LowerBodyController>(lowerBodyModel_);
+        
+        // 初始化状态向量
+        upperBodyState_.setZero(model::UpperBodyModel::NUM_JOINTS * 2); // 位置 + 速度
+        lowerBodyState_.setZero(model::LowerBodyModel::NUM_LEG_JOINTS * 2 + 
+                               model::LowerBodyModel::NUM_WHEELS); // 腿部位置+速度 + 轮子速度
+        
+        upperBodyCommand_.setZero(model::UpperBodyModel::NUM_JOINTS);
+        lowerBodyCommand_.setZero(model::LowerBodyModel::NUM_LEG_JOINTS + 
+                                 model::LowerBodyModel::NUM_WHEELS);
+        
+        RCLCPP_INFO(node_->get_logger(), "Robot control system initialized successfully");
+        return true;
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to initialize robot control system: %s", e.what());
+        return false;
+    }
+}
+
+void RobotControlSystem::start() {
+    // 设置控制定时器
+    auto period = std::chrono::duration<double>(1.0 / controlFrequency_);
+    controlTimer_ = node_->create_wall_timer(
+        period,
+        std::bind(&RobotControlSystem::controlCallback, this)
+    );
+    
+    RCLCPP_INFO(node_->get_logger(), "Robot control system started");
+}
+
+void RobotControlSystem::stop() {
+    controlTimer_->cancel();
+    upperBodyInterpolator_->stop();
+    lowerBodyInterpolator_->stop();
+    
+    // 停止所有运动
+    upperBodyController_->reset();
+    lowerBodyController_->reset();
+    
+    RCLCPP_INFO(node_->get_logger(), "Robot control system stopped");
+}
+
+void RobotControlSystem::update() {
+    // 在实际实现中，这里会从硬件接口读取当前状态
+    // 这里仅作为示例
+}
+
+state_machine::RobotState RobotControlSystem::getCurrentState() const {
+    return stateMachine_->getCurrentState();
+}
+
+void RobotControlSystem::handleMotionGoal(const ros::MotionGoal& goal) {
+    RCLCPP_INFO(node_->get_logger(), "Received motion goal: %s", 
+               stateMachine_->eventToString(static_cast<state_machine::Event>(goal.type)).c_str());
+    
+    // 根据目标类型转换为相应事件
+    state_machine::Event event;
+    switch (goal.type) {
+        case ros::MotionGoal::Type::MOVE_BASE:
+            event = state_machine::Event::START_MOVE_BASE;
+            // 设置基座移动目标
+            // ...
+            break;
+        case ros::MotionGoal::Type::ARM_MOTION:
+            event = state_machine::Event::START_ARM_MOTION;
+            // 设置手臂运动目标
+            upperBodyController_->setArmTargetPosition(
+                goal.target.arm.leftArm, 
+                goal.target.arm.position
+            );
+            break;
+        case ros::MotionGoal::Type::LEG_MOTION:
+            event = state_machine::Event::START_LEG_MOTION;
+            // 设置腿部运动目标
+            lowerBodyController_->setLegTargetPosition(
+                goal.target.leg.legIndex,
+                goal.target.leg.position
+            );
+            break;
+        case ros::MotionGoal::Type::BODY_POSE:
+            event = state_machine::Event::START_BODY_ADJUST;
+            // 设置身体姿态目标
+            // ...
+            break;
+        default:
+            RCLCPP_WARN(node_->get_logger(), "Unknown motion goal type");
+            return;
+    }
+    
+    // 处理事件
+    stateMachine_->handleEvent(event);
+}
+
+void RobotControlSystem::handleMotionCancel() {
+    RCLCPP_INFO(node_->get_logger(), "Received motion cancel request");
+    stateMachine_->handleEvent(state_machine::Event::STOP_MOTION);
+}
+
+void RobotControlSystem::registerStateCallbacks() {
+    // 注册状态进入回调
+    stateMachine_->registerEnterCallback(state_machine::RobotState::IDLE, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering IDLE state");
+        upperBodyController_->reset();
+        lowerBodyController_->reset();
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::MOVING_BASE, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering MOVING_BASE state");
+        // 启动基座移动控制
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::ARM_MOTION, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering ARM_MOTION state");
+        // 启动手臂运动控制
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::LEG_MOTION, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering LEG_MOTION state");
+        // 启动腿部运动控制
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::ERROR, [this]() {
+        RCLCPP_ERROR(node_->get_logger(), "Entering ERROR state");
+        // 错误处理
+    });
+    
+    // 注册状态退出回调
+    stateMachine_->registerExitCallback(state_machine::RobotState::MOVING_BASE, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Exiting MOVING_BASE state");
+    });
+    
+    // 其他状态回调...
+}
+
+void RobotControlSystem::controlCallback() {
+    // 更新系统状态
+    update();
+    
+    // 获取当前时间
+    const double t = node_->now().seconds();
+    
+    // 计算控制命令
+    upperBodyCommand_ = upperBodyController_->computeControl(t, upperBodyState_);
+    lowerBodyCommand_ = lowerBodyController_->computeControl(t, lowerBodyState_);
+    
+    // 检查运动是否完成
+    if (upperBodyController_->isGoalReached(upperBodyState_) &&
+        lowerBodyController_->isGoalReached(lowerBodyState_)) {
+        stateMachine_->handleEvent(state_machine::Event::MOTION_COMPLETED);
+    }
+    
+    // 发布反馈
+    publishFeedback();
+}
+
+void RobotControlSystem::publishFeedback() {
+    ros::MotionFeedback feedback;
+    
+    // 计算进度 (示例实现)
+    feedback.progress = 0.5; // 在实际实现中应根据实际情况计算
+    
+    // 设置当前状态
+    feedback.currentState = Eigen::VectorXd::Zero(
+        upperBodyState_.size() + lowerBodyState_.size()
+    );
+    feedback.currentState << upperBodyState_, lowerBodyState_;
+    
+    // 设置状态描述
+    feedback.status = stateMachine_->stateToString(getCurrentState());
+    
+    // 发布反馈
+    motionAction_->publishFeedback(feedback);
+}
+
+void RobotControlSystem::onStateTransition(state_machine::RobotState newState) {
+    RCLCPP_INFO(node_->get_logger(), "State transition to: %s", 
+               stateMachine_->stateToString(newState).c_str());
+}
+
+} // namespace robot_control

src/robot_control/control/LegController.cpp

@@ -0,0 +1,183 @@
+#include "robot_control/lower_body_controller.h"
+
+namespace robot_control {
+
+LowerBodyController::LowerBodyController(LowerBodyModel* model)
+    : model_(model), wheel_mode_(WheelMode::OMNI), gait_type_(GaitType::STAND) {
+    if (!model_) {
+        RCLCPP_ERROR(rclcpp::get_logger("LowerBodyController"), "无效的下肢模型指针");
+        throw std::invalid_argument("无效的下肢模型指针");
+    }
+    
+    // 初始化足端目标位置（站立姿态）
+    for (int i = 0; i < 4; ++i) {
+        foot_target_positions_[i] = model_->getFootPosition(i);
+    }
+    
+    // 初始化OCS2控制器
+    initOCS2Controller();
+}
+
+void LowerBodyController::initOCS2Controller() {
+    // 计算总自由度
+    const size_t leg_dof = model_->getLegJointIndices().size() * 3; // 4条腿，每条3个自由度
+    const size_t wheel_dof = model_->getWheelJointIndices().size();
+    const size_t state_dim = leg_dof + wheel_dof;
+    const size_t input_dim = state_dim;
+    
+    // 创建OCS2问题
+    problem_.reset(new ocs2::OptimalControlProblem);
+    
+    // 设置成本函数
+    problem_->costPtr->add("tracking_cost", std::make_unique<ocs2::QuadraticTrackingCost>(
+        state_dim, input_dim, 
+        Eigen::VectorXd::Zero(state_dim),  // 目标状态
+        Eigen::VectorXd::Zero(input_dim),   // 目标输入
+        Eigen::MatrixXd::Identity(state_dim),  // 状态权重
+        Eigen::MatrixXd::Identity(input_dim)   // 输入权重
+    ));
+    
+    // 添加重心约束
+    problem_->equalityConstraintPtr->add("com_constraint", 
+        std::make_unique<ocs2::CoMConstraint>(state_dim, input_dim));
+    
+    // 初始化求解器
+    solver_.reset(new ocs2::SLQ);
+    solver_->setProblem(*problem_);
+    
+    // 设置求解器参数
+    ocs2::SLQ_Settings settings;
+    settings.maxIterations = 100;
+    settings.convergenceTolerance = 1e-3;
+    solver_->setSettings(settings);
+    
+    RCLCPP_INFO(rclcpp::get_logger("LowerBodyController"), "OCS2控制器初始化完成，状态维度: %zu, 输入维度: %zu",
+                state_dim, input_dim);
+}
+
+void LowerBodyController::setGaitType(GaitType type) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    gait_type_ = type;
+    
+    // 根据步态类型重置控制器
+    if (gait_type_ == GaitType::WHEELED) {
+        // 轮式模式下，设置腿部为支撑姿态
+        setLegsToWheelMode();
+    }
+}
+
+void LowerBodyController::setLegsToWheelMode() {
+    // 设置腿部到适合轮式移动的姿态
+    for (int i = 0; i < 4; ++i) {
+        Eigen::Vector3d wheel_mode_pos = model_->getFootPosition(i);
+        wheel_mode_pos.z() -= 0.05;  // 稍微抬高一点，确保轮子着地
+        foot_target_positions_[i] = wheel_mode_pos;
+        
+        // 计算IK并更新目标关节角度
+        Eigen::VectorXd target_joints;
+        model_->legIK(i, wheel_mode_pos, target_joints);
+    }
+}
+
+void LowerBodyController::setWheelVelocity(const Eigen::Vector3d& vel_cmd) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    // 根据轮子模式计算各轮速度
+    if (wheel_mode_ == WheelMode::OMNI) {
+        // 全向轮运动学
+        double vx = vel_cmd(0);
+        double vy = vel_cmd(1);
+        double wz = vel_cmd(2);
+        
+        // 假设轮子布局为矩形
+        const double L = 0.3;  // 前后轮距离的一半
+        const double W = 0.2;  // 左右轮距离的一半
+        
+        // 计算四个轮子的目标速度
+        wheel_target_velocities_[0] = (vx - vy - wz * (L + W)) / model_->getWheelRadius();  // 前左
+        wheel_target_velocities_[1] = (vx + vy + wz * (L + W)) / model_->getWheelRadius();  // 前右
+        wheel_target_velocities_[2] = (vx + vy - wz * (L + W)) / model_->getWheelRadius();  // 后左
+        wheel_target_velocities_[3] = (vx - vy + wz * (L + W)) / model_->getWheelRadius();  // 后右
+    } else if (wheel_mode_ == WheelMode::DIFFERENTIAL) {
+        // 差速驱动
+        double v = vel_cmd(0);
+        double wz = vel_cmd(2);
+        
+        wheel_target_velocities_[0] = (v - wz * model_->getTrackWidth() / 2) / model_->getWheelRadius();  // 左
+        wheel_target_velocities_[1] = (v + wz * model_->getTrackWidth() / 2) / model_->getWheelRadius();  // 右
+        wheel_target_velocities_[2] = wheel_target_velocities_[0];  // 后左
+        wheel_target_velocities_[3] = wheel_target_velocities_[1];  // 后右
+    }
+}
+
+void LowerBodyController::setFootTargets(const std::vector<Eigen::Vector3d>& positions) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (positions.size() != 4) {
+        RCLCPP_ERROR(rclcpp::get_logger("LowerBodyController"), "足端目标数量必须为4");
+        return;
+    }
+    
+    for (int i = 0; i < 4; ++i) {
+        foot_target_positions_[i] = positions[i];
+        
+        // 计算IK并更新目标关节角度
+        Eigen::VectorXd target_joints;
+        model_->legIK(i, positions[i], target_joints);
+    }
+}
+
+void LowerBodyController::computeControl(const Eigen::VectorXd& leg_joints, 
+                                        const Eigen::VectorXd& leg_velocities,
+                                        const Eigen::VectorXd& wheel_joints,
+                                        const Eigen::VectorXd& wheel_velocities,
+                                        Eigen::VectorXd& leg_commands,
+                                        Eigen::VectorXd& wheel_commands) {
+    if (!isActive()) {
+        leg_commands.setZero(leg_joints.size());
+        wheel_commands.setZero(wheel_joints.size());
+        return;
+    }
+    
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    // 根据步态类型计算不同的控制命令
+    if (gait_type_ == GaitType::WHEELED) {
+        // 轮式模式：主要控制轮子，腿部保持固定姿态
+        computeWheelControl(wheel_joints, wheel_velocities, wheel_commands);
+        computeLegControl(leg_joints, leg_velocities, leg_commands);
+    } else {
+        // 步行模式：主要控制腿部，轮子不动
+        wheel_commands.setZero(wheel_joints.size());
+        computeLegControl(leg_joints, leg_velocities, leg_commands);
+    }
+}
+
+void LowerBodyController::computeLegControl(const Eigen::VectorXd& joints, 
+                                           const Eigen::VectorXd& velocities,
+                                           Eigen::VectorXd& commands) {
+    // 准备状态向量 [关节位置; 关节速度]
+    const size_t nq = joints.size();
+    Eigen::VectorXd state(2 * nq);
+    state << joints, velocities;
+    
+    // 求解最优控制
+    ocs2::scalar_t t = 0.0;  // 当前时间
+    ocs2::vector_t optimal_input;
+    solver_->computeInput(t, state, optimal_input);
+    
+    // 将最优输入作为关节指令
+    commands = optimal_input;
+}
+
+void LowerBodyController::computeWheelControl(const Eigen::VectorXd& joints, 
+                                             const Eigen::VectorXd& velocities,
+                                             Eigen::VectorXd& commands) {
+    // 简单的PD控制
+    commands.resize(velocities.size());
+    for (size_t i = 0; i < velocities.size() && i < wheel_target_velocities_.size(); ++i) {
+        commands(i) = kp_wheels_(i) * (wheel_target_velocities_[i] - velocities(i));
+    }
+}
+
+} // namespace robot_control
+    

src/robot_control/model/RobotModel.cpp

@@ -0,0 +1,52 @@
+#include "robot_control/robot_model.h"
+
+namespace robot_control {
+
+RobotModel::RobotModel(const std::string& urdf_path) {
+    // 加载URDF模型
+    pinocchio::urdf::buildModel(urdf_path, model_);
+    data_ = std::make_unique<pinocchio::Data>(model_);
+    
+    // 初始化上下身模型
+    upper_body_ = std::make_unique<UpperBodyModel>(model_, data_.get());
+    lower_body_ = std::make_unique<LowerBodyModel>(model_, data_.get());
+    
+    RCLCPP_INFO(rclcpp::get_logger("RobotModel"), "机器人模型加载成功，关节数量: %d", model_.nq);
+}
+
+void RobotModel::update(const Eigen::VectorXd& q, const Eigen::VectorXd& dq) {
+    // 更新机器人状态
+    pinocchio::forwardKinematics(model_, *data_, q, dq);
+    pinocchio::updateFramePlacements(model_, *data_);
+    
+    // 更新上下身模型
+    upper_body_->update(q, dq);
+    lower_body_->update(q, dq);
+}
+
+const Eigen::VectorXd& RobotModel::getJointPositions() const {
+    return joint_positions_;
+}
+
+const Eigen::VectorXd& RobotModel::getJointVelocities() const {
+    return joint_velocities_;
+}
+
+UpperBodyModel* RobotModel::getUpperBody() {
+    return upper_body_.get();
+}
+
+LowerBodyModel* RobotModel::getLowerBody() {
+    return lower_body_.get();
+}
+
+const pinocchio::Model& RobotModel::getPinocchioModel() const {
+    return model_;
+}
+
+pinocchio::Data* RobotModel::getPinocchioData() {
+    return data_.get();
+}
+
+} // namespace robot_control
+    

src/robot_control/planning/RealtimeINterpolation.cpp

@@ -0,0 +1,310 @@
+#include "robot_control/realtime_interpolator.h"
+#include <ocs2_core/misc/LinearInterpolation.h>
+#include <ocs2_core/misc/Algebra.h>
+#include <rclcpp/rclcpp.hpp>
+#include <chrono>
+#include <algorithm>
+
+namespace robot_control {
+namespace planning {
+
+RealtimeInterpolator::RealtimeInterpolator(double controlFrequency) 
+    : controlFrequency_(controlFrequency),
+      dt_(1.0 / controlFrequency),
+      running_(true),
+      isActive_(false),
+      isGoalReached_(true),
+      interpolationType_(InterpolationType::CUBIC) {
+    // 启动插值线程
+    interpolationThread_ = std::thread(&RealtimeInterpolator::interpolationThread, this);
+}
+
+RealtimeInterpolator::~RealtimeInterpolator() {
+    stop();
+    if (interpolationThread_.joinable()) {
+        interpolationThread_.join();
+    }
+}
+
+void RealtimeInterpolator::setTargetTrajectories(const ocs2::TargetTrajectories& trajectories, 
+                                                InterpolationType type) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    // 检查轨迹是否有效
+    if (trajectories.timeTrajectory.empty() || trajectories.stateTrajectory.empty()) {
+        RCLCPP_WARN(rclcpp::get_logger("RealtimeInterpolator"), "设置了空轨迹");
+        return;
+    }
+    
+    // 确保时间轨迹单调递增
+    bool timeValid = true;
+    for (size_t i = 1; i < trajectories.timeTrajectory.size(); ++i) {
+        if (trajectories.timeTrajectory[i] <= trajectories.timeTrajectory[i-1]) {
+            timeValid = false;
+            break;
+        }
+    }
+    
+    if (!timeValid) {
+        RCLCPP_ERROR(rclcpp::get_logger("RealtimeInterpolator"), "时间轨迹必须单调递增");
+        return;
+    }
+    
+    trajectories_ = trajectories;
+    interpolationType_ = type;
+    isActive_ = true;
+    isGoalReached_ = false;
+    
+    // 通知插值线程有新轨迹
+    cv_.notify_one();
+}
+
+bool RealtimeInterpolator::getCurrentReference(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    if (!isActive_ || trajectories_.timeTrajectory.empty()) {
+        return false;
+    }
+    
+    // 检查是否超出轨迹时间范围
+    const double t_start = trajectories_.timeTrajectory.front();
+    const double t_end = trajectories_.timeTrajectory.back();
+    
+    if (t < t_start) {
+        x_ref = trajectories_.stateTrajectory.front();
+        u_ref = trajectories_.inputTrajectory.empty() ? ocs2::vector_t::Zero(0) : trajectories_.inputTrajectory.front();
+        return true;
+    } else if (t > t_end) {
+        x_ref = trajectories_.stateTrajectory.back();
+        u_ref = trajectories_.inputTrajectory.empty() ? ocs2::vector_t::Zero(0) : trajectories_.inputTrajectory.back();
+        isGoalReached_ = true;
+        return true;
+    }
+    
+    // 根据当前插值类型计算参考点
+    interpolate(t, x_ref, u_ref);
+    return true;
+}
+
+void RealtimeInterpolator::stop() {
+    running_ = false;
+    isActive_ = false;
+    cv_.notify_one();
+}
+
+void RealtimeInterpolator::setControlCallback(ControlCallback callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    controlCallback_ = callback;
+}
+
+void RealtimeInterpolator::interpolationThread() {
+    rclcpp::Time startTime = rclcpp::Clock().now();
+    
+    while (running_) {
+        // 等待新轨迹或定期唤醒
+        std::unique_lock<std::mutex> lock(mutex_);
+        cv_.wait_for(lock, std::chrono::duration<double>(dt_));
+        
+        if (!running_) {
+            break;
+        }
+        
+        if (isActive_ && !trajectories_.timeTrajectory.empty()) {
+            // 计算当前时间（相对于轨迹开始时间）
+            const double currentTime = (rclcpp::Clock().now() - startTime).seconds();
+            
+            // 获取当前参考点
+            ocs2::vector_t x_ref, u_ref;
+            bool valid = getCurrentReference(currentTime, x_ref, u_ref);
+            
+            // 检查是否到达目标
+            if (isGoalReached_) {
+                isActive_ = false;
+            }
+            
+            // 如果有回调函数且参考点有效，则调用回调
+            if (valid && controlCallback_) {
+                controlCallback_(currentTime, x_ref, u_ref);
+            }
+        }
+    }
+}
+
+void RealtimeInterpolator::interpolate(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    switch (interpolationType_) {
+        case InterpolationType::LINEAR:
+            linearInterpolation(t, x_ref, u_ref);
+            break;
+        case InterpolationType::CUBIC:
+            cubicInterpolation(t, x_ref, u_ref);
+            break;
+        case InterpolationType::QUINTIC:
+            quinticInterpolation(t, x_ref, u_ref);
+            break;
+        default:
+            linearInterpolation(t, x_ref, u_ref);
+    }
+}
+
+void RealtimeInterpolator::linearInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    // 使用OCS2的线性插值工具
+    const auto& time = trajectories_.timeTrajectory;
+    const auto& state = trajectories_.stateTrajectory;
+    const auto& input = trajectories_.inputTrajectory;
+    
+    size_t index;
+    double alpha;
+    ocs2::LinearInterpolation::interpolationIndices(time, t, index, alpha);
+    
+    x_ref = (1 - alpha) * state[index] + alpha * state[index + 1];
+    
+    if (!input.empty()) {
+        u_ref = (1 - alpha) * input[index] + alpha * input[index + 1];
+    } else {
+        u_ref = ocs2::vector_t::Zero(0);
+    }
+}
+
+void RealtimeInterpolator::cubicInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    const auto& time = trajectories_.timeTrajectory;
+    const auto& state = trajectories_.stateTrajectory;
+    const auto& input = trajectories_.inputTrajectory;
+    
+    if (time.size() < 2) {
+        linearInterpolation(t, x_ref, u_ref);
+        return;
+    }
+    
+    size_t index;
+    double alpha;
+    ocs2::LinearInterpolation::interpolationIndices(time, t, index, alpha);
+    
+    const double t0 = time[index];
+    const double t1 = time[index + 1];
+    const double dt = t1 - t0;
+    const double s = (t - t0) / dt;  // 标准化时间 [0, 1]
+    
+    // 三次多项式系数: a*s³ + b*s² + c*s + d
+    const ocs2::vector_t x0 = state[index];
+    const ocs2::vector_t x1 = state[index + 1];
+    
+    // 计算导数（速度）
+    ocs2::vector_t dx0, dx1;
+    
+    if (index > 0) {
+        dx0 = (x1 - state[index - 1]) / (t1 - time[index - 1]);
+    } else {
+        dx0 = (x1 - x0) / dt;  // 起点使用前向差分
+    }
+    
+    if (index + 1 < time.size() - 1) {
+        dx1 = (state[index + 2] - x0) / (time[index + 2] - t0);
+    } else {
+        dx1 = (x1 - x0) / dt;  // 终点使用后向差分
+    }
+    
+    // 三次多项式插值
+    const double s2 = s * s;
+    const double s3 = s2 * s;
+    
+    x_ref = (2*s3 - 3*s2 + 1) * x0 + 
+            (s3 - 2*s2 + s) * dt * dx0 + 
+            (-2*s3 + 3*s2) * x1 + 
+            (s3 - s2) * dt * dx1;
+    
+    // 输入插值使用线性插值
+    if (!input.empty()) {
+        u_ref = (1 - alpha) * input[index] + alpha * input[index + 1];
+    } else {
+        u_ref = ocs2::vector_t::Zero(0);
+    }
+}
+
+void RealtimeInterpolator::quinticInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    const auto& time = trajectories_.timeTrajectory;
+    const auto& state = trajectories_.stateTrajectory;
+    const auto& input = trajectories_.inputTrajectory;
+    
+    if (time.size() < 2) {
+        linearInterpolation(t, x_ref, u_ref);
+        return;
+    }
+    
+    size_t index;
+    double alpha;
+    ocs2::LinearInterpolation::interpolationIndices(time, t, index, alpha);
+    
+    const double t0 = time[index];
+    const double t1 = time[index + 1];
+    const double dt = t1 - t0;
+    const double s = (t - t0) / dt;  // 标准化时间 [0, 1]
+    
+    // 五次多项式需要位置、速度和加速度信息
+    const ocs2::vector_t x0 = state[index];
+    const ocs2::vector_t x1 = state[index + 1];
+    
+    // 计算速度 (一阶导数)
+    ocs2::vector_t dx0, dx1;
+    // 计算加速度 (二阶导数)
+    ocs2::vector_t ddx0, ddx1;
+    
+    // 计算速度
+    if (index > 0) {
+        dx0 = (x1 - state[index - 1]) / (t1 - time[index - 1]);
+    } else {
+        dx0 = (x1 - x0) / dt;
+    }
+    
+    if (index + 1 < time.size() - 1) {
+        dx1 = (state[index + 2] - x0) / (time[index + 2] - t0);
+    } else {
+        dx1 = (x1 - x0) / dt;
+    }
+    
+    // 计算加速度
+    if (index > 1) {
+        ddx0 = (dx0 - (x0 - state[index - 2]) / (t0 - time[index - 2])) / 
+               ((t0 - time[index - 2]) / 2.0);
+    } else {
+        ddx0 = ocs2::vector_t::Zero(x0.size());
+    }
+    
+    if (index + 2 < time.size() - 1) {
+        ddx1 = ((state[index + 2] - x1) / (time[index + 2] - t1) - dx1) / 
+               ((time[index + 2] - t1) / 2.0);
+    } else {
+        ddx1 = ocs2::vector_t::Zero(x1.size());
+    }
+    
+    // 五次多项式系数计算
+    const double s2 = s * s;
+    const double s3 = s2 * s;
+    const double s4 = s3 * s;
+    const double s5 = s4 * s;
+    
+    // 五次多项式基函数
+    const double a0 = 1.0 - 10.0*s3 + 15.0*s4 - 6.0*s5;
+    const double a1 = s - 6.0*s3 + 8.0*s4 - 3.0*s5;
+    const double a2 = 0.5*s2 - 1.5*s3 + 1.5*s4 - 0.5*s5;
+    const double b0 = 10.0*s3 - 15.0*s4 + 6.0*s5;
+    const double b1 = -4.0*s3 + 7.0*s4 - 3.0*s5;
+    const double b2 = 0.5*s3 - 1.0*s4 + 0.5*s5;
+    
+    // 计算插值位置
+    x_ref = a0 * x0 + 
+            a1 * dt * dx0 + 
+            a2 * dt*dt * ddx0 + 
+            b0 * x1 + 
+            b1 * dt * dx1 + 
+            b2 * dt*dt * ddx1;
+    
+    // 输入插值使用线性插值
+    if (!input.empty()) {
+        u_ref = (1 - alpha) * input[index] + alpha * input[index + 1];
+    } else {
+        u_ref = ocs2::vector_t::Zero(0);
+    }
+}
+
+} // namespace planning
+} // namespace robot_control

src/robot_control/state_machine/RobotStateMachine.cpp

@@ -0,0 +1,113 @@
+#include "robot_control/state_machine.h"
+
+namespace robot_control {
+
+StateMachine::StateMachine() : current_state_(RobotState::INITIALIZING) {
+    RCLCPP_INFO(rclcpp::get_logger("StateMachine"), "状态机初始化，初始状态: %s", stateToString(current_state_).c_str());
+}
+
+void StateMachine::handleEvent(RobotEvent event) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    RobotState new_state = current_state_;
+    
+    // 根据当前状态和事件确定新状态
+    switch (current_state_) {
+        case RobotState::INITIALIZING:
+            if (event == RobotEvent::INIT_COMPLETE) {
+                new_state = RobotState::STANDBY;
+            }
+            break;
+            
+        case RobotState::STANDBY:
+            if (event == RobotEvent::START_MOTION) {
+                new_state = RobotState::MOVING;
+            } else if (event == RobotEvent::ERROR_OCCURRED) {
+                new_state = RobotState::ERROR;
+            }
+            break;
+            
+        case RobotState::MOVING:
+            if (event == RobotEvent::MOTION_COMPLETE) {
+                new_state = RobotState::STANDBY;
+            } else if (event == RobotEvent::STOP_MOTION) {
+                new_state = RobotState::STANDBY;
+            } else if (event == RobotEvent::ERROR_OCCURRED) {
+                new_state = RobotState::ERROR;
+            } else if (event == RobotEvent::EMERGENCY_STOP) {
+                new_state = RobotState::ESTOP;
+            }
+            break;
+            
+        case RobotState::ERROR:
+            if (event == RobotEvent::ERROR_RESOLVED) {
+                new_state = RobotState::STANDBY;
+            } else if (event == RobotEvent::EMERGENCY_STOP) {
+                new_state = RobotState::ESTOP;
+            }
+            break;
+            
+        case RobotState::ESTOP:
+            if (event == RobotEvent::ESTOP_RELEASED) {
+                new_state = RobotState::INITIALIZING;
+            }
+            break;
+    }
+    
+    // 如果状态发生变化，调用状态回调
+    if (new_state != current_state_) {
+        RCLCPP_INFO(rclcpp::get_logger("StateMachine"), "状态转换: %s -> %s", 
+                   stateToString(current_state_).c_str(), 
+                   stateToString(new_state).c_str());
+        
+        // 调用退出当前状态的回调
+        if (exit_state_callbacks_[current_state_]) {
+            exit_state_callbacks_[current_state_]();
+        }
+        
+        current_state_ = new_state;
+        
+        // 调用进入新状态的回调
+        if (enter_state_callbacks_[current_state_]) {
+            enter_state_callbacks_[current_state_]();
+        }
+        
+        // 调用通用状态变化回调
+        if (state_change_callback_) {
+            state_change_callback_(current_state_);
+        }
+    }
+}
+
+RobotState StateMachine::getCurrentState() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return current_state_;
+}
+
+std::string StateMachine::stateToString(RobotState state) {
+    switch (state) {
+        case RobotState::INITIALIZING: return "INITIALIZING";
+        case RobotState::STANDBY: return "STANDBY";
+        case RobotState::MOVING: return "MOVING";
+        case RobotState::ERROR: return "ERROR";
+        case RobotState::ESTOP: return "ESTOP";
+        default: return "UNKNOWN";
+    }
+}
+
+void StateMachine::setEnterStateCallback(RobotState state, std::function<void()> callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    enter_state_callbacks_[state] = callback;
+}
+
+void StateMachine::setExitStateCallback(RobotState state, std::function<void()> callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    exit_state_callbacks_[state] = callback;
+}
+
+void StateMachine::setStateChangeCallback(std::function<void(RobotState)> callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state_change_callback_ = callback;
+}
+
+} // namespace robot_control
+