how to get yaw from quaternion values?
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].
Hi Tomm I would like to ask how were u able to achieve it using Navigation and actionlib. I am working on a similar project and am stuck and have less time, so i can't read from scratch. If you can walk me through the steps and probably share your code, it would be great help Thanks
@vsd, you should ask a new question to get some help with your problem.