add gyro value

src/robot_control/include/extended_kalman_filter.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <Eigen/Dense>
+#include <fstream>  // 用于保存轨迹数据
+
+namespace robot_control {
+class EKF 
+{
+private:
+    double dt;
+    
+    // 卡尔曼滤波参数
+    Eigen::Matrix4d Q;        // 过程噪声协方差矩阵
+    Eigen::MatrixXd R;        // 测量噪声协方差矩阵
+    
+    // 状态向量 [x, y, θ, ω]
+    Eigen::Vector4d X;
+    // 协方差矩阵
+    Eigen::Matrix4d P;
+    
+    // 角度归一化到[-π, π]
+    double normalizeAngle(double angle) {
+        while (angle > M_PI) angle -= 2.0 * M_PI;
+        while (angle < -M_PI) angle += 2.0 * M_PI;
+        return angle;
+    }
+
+public:
+    EKF(double init_x = 0, double init_y = 0, double init_theta = 0, double init_omega = 0);
+    
+    // 预测步骤
+    void predict(double v, double omega_wheel);
+    
+    // 更新步骤
+    void update(double gyro_omega);
+    
+    // 获取当前状态
+    Eigen::Vector4d getState() { return X; }
+};
+}

src/robot_control/include/robot_control_manager.hpp

@@ -1,9 +1,11 @@
 #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"
@@ -13,8 +15,10 @@
 #include "waist_control.hpp"
 
 #include "interfaces/msg/motor_pos.hpp"
+#include "interfaces/msg/imu_msg.hpp"
 
 using MotorPos = interfaces::msg::MotorPos;
+using ImuMsg = interfaces::msg::ImuMsg;
 
 namespace robot_control
 {
@@ -33,9 +37,9 @@ namespace robot_control
             void JogAxis(size_t axis, int dir);
 
             // 检查参数是否合理
-            bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle){return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);};
-            bool SetMoveLegParameters(double moveLegDistance){return legController_->SetMoveLegParametersInternal(moveLegDistance);};
-            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);};
+            bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
+            bool SetMoveLegParameters(double moveLegDistance);
+            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
             
             // 机器人关节指令
             std_msgs::msg::Float64MultiArray GetJointCommands();
@@ -43,9 +47,11 @@ namespace robot_control
             std_msgs::msg::Float64MultiArray GetWheelCommands();
             std_msgs::msg::Float64MultiArray GetWheelFeedback();
 
+            // 机器人状态
             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();
 
@@ -81,6 +87,12 @@ namespace robot_control
 
             std::vector<bool> jointInited_; // 机器人是否已经初始化
             bool isJointInitValueSet_;
+
+            // 陀螺仪状态
+            ImuMsg imuMsg_;
+            std::vector<double> gyroValues_; // 陀螺仪数据（弧度/秒）
+            std::vector<double> gyroVelocities_; // 陀螺仪速度（弧度/秒）
+            std::vector<double> gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2）
             
             // 临时变量
             std::vector<double> tempWaistCmd_;
@@ -94,6 +106,9 @@ namespace robot_control
 
             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

src/robot_control/include/robot_control_node.hpp

@@ -17,7 +17,6 @@
 #include "interfaces/action/move_wheel.hpp"
 #include "interfaces/msg/motor_cmd.hpp"
 
-
 using MoveHome = interfaces::action::MoveHome;
 using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
 
@@ -81,6 +80,7 @@ namespace robot_control
             rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
             rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
             rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
+            rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;
 
             RobotControlManager robotControlManager_;
 
@@ -102,6 +102,7 @@ namespace robot_control
             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

src/robot_control/src/extended_kalman_filter.cpp

@@ -0,0 +1,73 @@
+#include "extended_kalman_filter.hpp"
+
+namespace robot_control {
+EKF::EKF(double init_x, double init_y, double init_theta, double init_omega) 
+{
+    this->dt = 0.001;
+    
+    // 初始化状态向量
+    X << init_x, init_y, init_theta, init_omega;
+    
+    // 初始化协方差矩阵
+    P = Eigen::Matrix4d::Zero();
+    P.diagonal() << 0.1, 0.1, 0.05, 0.01;
+}
+    
+    // 预测步骤
+void EKF::predict(double v, double omega_wheel) {
+    // double x = X(0);
+    // double y = X(1);
+    double theta = X(2);
+    // double omega = X(3);
+    
+    // 状态转移矩阵F
+    Eigen::Matrix4d F = Eigen::Matrix4d::Identity();
+    F(0, 2) = -v * sin(theta) * dt;
+    F(1, 2) = v * cos(theta) * dt;
+    F(2, 3) = dt;
+    
+    // 输入矩阵B
+    Eigen::Matrix<double, 4, 2> B;
+    B << cos(theta) * dt, 0,
+            sin(theta) * dt, 0,
+            0, dt,
+            0, 1;
+            
+    // 控制输入
+    Eigen::Vector2d u;
+    u << v, omega_wheel;
+    
+    // 预测状态
+    X = F * X + B * u;
+    // 预测协方差
+    P = F * P * F.transpose() + Q;
+    
+    // 归一化航向角
+    X(2) = normalizeAngle(X(2));
+}
+    
+// 更新步骤
+void EKF::update(double gyro_omega) {
+    // 观测矩阵H
+    Eigen::Matrix<double, 1, 4> H;
+    H << 0, 0, 0, 1;
+    
+    // 测量残差
+    Eigen::VectorXd z(1);
+    z << gyro_omega;
+    Eigen::VectorXd y = z - H * X;
+    
+    // 计算卡尔曼增益
+    Eigen::MatrixXd S = H * P * H.transpose() + R;
+    Eigen::MatrixXd K = P * H.transpose() * S.inverse();
+    
+    // 更新状态
+    X = X + K * y;
+    // 更新协方差
+    Eigen::Matrix4d I = Eigen::Matrix4d::Identity();
+    P = (I - K * H) * P;
+    
+    // 归一化航向角
+    X(2) = normalizeAngle(X(2));
+}
+}

src/robot_control/src/robot_control_manager.cpp

@@ -21,6 +21,10 @@ void RobotControlManager::init()
         jointCommands_.data.push_back(0.0);
     }
 
+    gyroValues_.resize(4, 0.0);
+    gyroVelocities_.resize(3, 0.0);
+    gyroAccelerations_.resize(3, 0.0);
+
     // Initialize the wheel commands
     for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
     {
@@ -76,6 +80,21 @@ RobotControlManager::~RobotControlManager()
     delete wheelController_;
 }
 
+bool RobotControlManager::SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle)
+{
+    return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);
+}
+
+bool RobotControlManager::SetMoveLegParameters(double moveLegDistance)
+{
+    return legController_->SetMoveLegParametersInternal(moveLegDistance);
+}
+
+bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double moveYawAngle)
+{
+    return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
+}
+
 Float64MultiArray RobotControlManager::GetWheelCommands()
 {
     return wheelCommands_;
@@ -157,6 +176,79 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
     return true;  // 所有元素都是 true
 }
 
+void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
+{
+    imuMsg_ = *msg;
+    sensor_msgs::msg::Imu tempMsg = imuMsg_.imu;
+
+    if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0)
+    {
+        return;
+    }
+
+    if (tempMsg.linear_acceleration.x == 0 && tempMsg.linear_acceleration.y == 0 && tempMsg.linear_acceleration.z == 0)
+    {
+        return;
+    }
+
+    QuaternionToRPYDeg(
+        tempMsg.orientation.w, 
+        tempMsg.orientation.x, 
+        tempMsg.orientation.y, 
+        tempMsg.orientation.z, 
+        gyroValues_[0], 
+        gyroValues_[1], 
+        gyroValues_[2]
+    );
+
+    gyroVelocities_[0] = tempMsg.angular_velocity.x;
+    gyroVelocities_[1] = tempMsg.angular_velocity.y;
+    gyroVelocities_[2] = tempMsg.angular_velocity.z;
+
+    gyroAccelerations_[0] = tempMsg.linear_acceleration.x;
+    gyroAccelerations_[1] = tempMsg.linear_acceleration.y;
+    gyroAccelerations_[2] = tempMsg.linear_acceleration.z;
+
+    std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl;
+
+}
+
+void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
+{
+    double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
+    if (norm < 1e-12) {  // 避免除以零
+        roll = 0;
+        pitch = 0;
+        yaw = 0;
+        return;
+    }
+
+    qw /= norm;
+    qx /= norm;
+    qy /= norm;
+    qz /= norm;
+    
+    // 计算Roll角（X轴）
+    roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy));
+    
+    // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]）
+    double sin_pitch = 2 * (qw*qy - qz*qx);
+    sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); 
+    pitch = asin(sin_pitch);
+    
+    // 计算Yaw角（Z轴）
+    yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz));
+}
+
+void RobotControlManager::QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
+{
+    QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);
+
+    roll = roll * 180.0 / M_PI;
+    pitch = pitch * 180.0 / M_PI;
+    yaw = yaw * 180.0 / M_PI;
+}
+
 void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg) 
 {
     motorPos_ = *msg;

src/robot_control/src/robot_control_node.cpp

@@ -75,6 +75,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
 
     motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
     ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
+    imuMsgSub_ = this->create_subscription<ImuMsg>("/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));
@@ -686,6 +687,16 @@ void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::S
     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) {  // 检查消息是否有效