使用 MAVROS 获取飞机偏航角并转化为东为原点，逆时针为正的 C++ 代码示例

以下是一个简单的 C++ 代码示例，用于使用 MAVROS 获取飞机的偏航角，并将其转化为以东为原点，逆时针为正的角度值。

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_datatypes.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandHome.h>
#include <sensor_msgs/Imu.h>
#include <cmath>

double yaw = 0.0;

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    tf::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    tf::Matrix3x3 m(q);
    double roll, pitch;
    m.getRPY(roll, pitch, yaw);
    yaw = fmod((yaw + 2*M_PI), (2*M_PI)) * 180/M_PI;  // 转化为角度，以东为原点，逆时针为正
    yaw = 90 - yaw;
    if (yaw < 0) {
        yaw += 360;
    }
    ROS_INFO("Yaw: [%f]", yaw);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "yaw_test");
    ros::NodeHandle nh;

    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, poseCallback);

    ros::spin();

    return 0;
}

在此代码中，我们使用了 MAVROS 的本地姿态话题（/mavros/local_position/pose）来获取飞机的姿态信息。当收到姿态信息时，我们使用 tf 库中的函数将四元数转化为欧拉角（roll、pitch 和 yaw）。然后，我们将偏航角转化为角度，并将其转化为以东为原点，逆时针为正的角度值。最后，我们打印出偏航角的值。

注意，这只是一个简单的示例代码，实际的应用可能需要更多的代码来实现所需的功能。