code optimization

imu_dev/src/main.cpp

@@ -1,7 +1,48 @@
 #include "rclcpp/rclcpp.hpp"
 #include "interfaces/msg/imu_msg.hpp"
 #include "sensor_msgs/msg/imu.hpp"
+#include <fstream>
 using namespace std;
+
+void 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));
+}
+
+// std::ofstream rpy_file("rpy.txt");
+void 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 / 3.1415926;
+    pitch = pitch * 180.0 / 3.1415926;
+    yaw = yaw * 180.0 / 3.1415926;
+    // rpy_file<<roll<<","<<pitch<<","<<yaw<<std::endl;
+    std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
+}
+
 int main(int argc, char **argv)
 {
   rclcpp::init(argc, argv);
@@ -16,6 +57,19 @@ int main(int argc, char **argv)
       imu_msg.position="front";
       imu_msg.type="None";
       imu_msg.imu=*msg;
+
+      double gyroValues_[3];
+
+      QuaternionToRPYDeg(
+        imu_msg.imu.orientation.w, 
+        imu_msg.imu.orientation.x, 
+        imu_msg.imu.orientation.y, 
+        imu_msg.imu.orientation.z, 
+        gyroValues_[0], 
+        gyroValues_[1], 
+        gyroValues_[2]
+      );
+
       imu_pub->publish(imu_msg);
   });
 ///livox/imu
@@ -35,4 +89,5 @@ int main(int argc, char **argv)
 
   rclcpp::spin(node);
   rclcpp::shutdown();
+  rpy_file.close();
 }