How to read Yaw from MAVROS?

[cnpcshangbo]

From this link https://github.com/mavlink/mavros/issues/169, we know topic ~local_position/pose (geometry_msgs/PoseStamped) has the info we need.

Point position
Quaternion orientation

geometry_msgs/Point position
geometry_msgs/Quaternion orientation

However, we need to get Yaw from Quaternion.

[cnpcshangbo]

So this is the code we need: https://answers.ros.org/question/238083/how-to-get-yaw-from-quaternion-values/

//Make sure you include all of them
#include "ros/ros.h"

//#include <ros/console.h>
#include "geometry_msgs/PoseStamped.h"
#include <tf/transform_datatypes.h>

//Create global variables and declare a function
double quatx;
double quaty;
double quatz;
double quatw;

void ComPoseCallback(const geometry_msgs::PoseStamped& msg);

//Add them to main function
int main(int argc, char **argv)
{
    ros::init(argc, argv, "Test_FK"); // connect to roscore
    ros::NodeHandle n;                                     // node object
    ros::Subscriber ComPose_sub = n.subscribe("/mavros/local_position/pose", 1000, ComPoseCallback);
    ros::spin();
}

void ComPoseCallback(const geometry_msgs::PoseStamped& msg)            
{
    ROS_INFO("Seq: [%d]", msg->header.seq);
    ROS_INFO("Position.x: [%f], y: [%f], z: [%f]", msg->pose.position.x,msg->pose.position.y, msg->pose.position.z);
    ROS_INFO("Orientation.x: [%f], y: [%f], z: [%f], w: [%f]", msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w);

   float linearposx=msg.pose.position.x;
   float linearposy=msg.pose.position.y;
   double quatx= msg.pose.orientation.x;
   double quaty= msg.pose.orientation.y;
   double quatz= msg.pose.orientation.z;
   double quatw= msg.pose.orientation.w;

    tf::Quaternion q(quatx, quaty, quatz, quatw);
    tf::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    ROS_INFO("Roll: [%f],Pitch: [%f],Yaw: [%f]",roll,pitch,yaw);
    return ;
}

[cnpcshangbo]

rostopic echo /mavros/local_position/pose

header: 
  seq: 3282
  stamp: 
    secs: 944
    nsecs: 447500000
  frame_id: fcu
pose: 
  position: 
    x: 0.0189292430878
    y: -0.000100355151517
    z: 4.06078195572
  orientation: 
    x: -0.000701458086
    y: 0.00197929425722
    z: 0.705501406061
    w: -0.708705408753