用mavros获得飞机float形式的偏航角并将其转化为东为原点逆时针为正用C++实现

以下是一个示例代码，其中假设偏航角的单位为弧度：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>

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转化为东为原点，逆时针为正
  yaw = -yaw + M_PI/2;
  if (yaw < 0) {
    yaw += 2*M_PI;
  }
}

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

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

  while (ros::ok()) {
    ROS_INFO("Yaw: %.2f", yaw);
    ros::spinOnce();
  }

  return 0;
}

在上述代码中，订阅了mavros提供的/mavros/local_position/pose话题，并在回调函数中将飞机的四元数转化为欧拉角，然后将偏航角转化为东为原点，逆时针为正。最后，使用ROS的ros::spinOnce()函数让节点在循环中保持运行