how to get yaw from quaternion values?

edit

Hello everyone,

I am new to ROS and this community. I am working with pioneer 3dx robot. My aim is to drive it in a square shape path and return back to its initial position. I am able to get quaternion values [x,y,z,w] from my robot. But for the purpose of turning the robot 90 degree I think I need rotation about z axis [yaw]. I have written the following code:

#include "ros/ros.h"

#include "nav_msgs/Odometry.h" 
#include <ros/console.h>
#include "geometry_msgs/Pose.h"
#include <tf/transform_datatypes.h>

double quatx;
double quaty;
double quatz;
double quatw;


void ComPoseCallback(const nav_msgs::Odometry::ConstPtr& msg);


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("/RosAria/pose", 1000, ComPoseCallback);
    ros::spin();
}


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

   float linearposx=msg->pose.pose.position.x;
   float linearposy=msg->pose.pose.position.y;
   double quatx= msg->pose.pose.orientation.x;
   double quaty= msg->pose.pose.orientation.y;
   double quatz= msg->pose.pose.orientation.z;
   double quatw= msg->pose.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 ;
}

I code compiles and do return some values. I was just spinning the robot in anti clockwise direction. Here are the values:

Position-> x: [0.000000], y: [-0.001000], z: [0.000000] Orientation-> x: [0.000000], y: [0.000000], z: [-0.054026], w: [0.998540] Roll: [0.000000],Pitch: [0.000000],Yaw: [-0.10810]

Position-> x: [0.000000], y: [-0.001000], z: [0.000000] Orientation-> x: [0.000000], y: [0.000000], z: [-0.055554], w: [0.998456] Roll: [0.000000],Pitch: [0.000000],Yaw: [-0.111165]

Position-> x: [0.000000], y: [-0.001000], z: [0.000000] Orientation-> x: [0.000000], y: [0.000000], z: [-0.055554], w: [0.998456] Roll: [0.000000],Pitch: [0.000000],Yaw: [-0.111165]

I don't understand whether i am getting the correct values or not. To my understanding Yaw is in radians and must increase with time [spinning at one posiiton].