Ask Your Question
1

how to get yaw from quaternion values?

asked 2016-06-26 16:52:03 -0500

Tomm gravatar image

updated 2016-06-26 17:00:12 -0500

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].

edit retag flag offensive close merge delete

Comments

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 gravatar image vsd  ( 2017-06-28 23:45:08 -0500 )edit

@vsd, you should ask a new question to get some help with your problem.

jayess gravatar image jayess  ( 2017-07-31 10:56:17 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2016-06-27 02:30:56 -0500

Humpelstilzchen gravatar image

updated 2016-06-27 02:32:20 -0500

Spinning counter clockwise yaw should increase, yes, to convert check this answer.

If you want to move in a square you can easily use the Navigation & ActionLib to move your robot to e.g. (1,0), (1,-1), (0,1), (0, 0). It will then rotate automatically.

edit flag offensive delete link more

Comments

Navigation & ActionLib is an easier way to do it. Thanks a lot.

Tomm gravatar image Tomm  ( 2016-06-27 14:04:14 -0500 )edit

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 gravatar image vsd  ( 2017-06-27 02:44:59 -0500 )edit
0

answered 2017-08-09 12:25:13 -0500

Hi Tomm,

I had a similar problem, to convert quaternions to RPY. In my case, I was working with turtlebot, but it's also a ground robot and I've simplified the problem to a 3 DoF robot (planar scenarios).

Basically, the magic happens here:

tf::Quaternion q(
    msg->pose.pose.orientation.x,
    msg->pose.pose.orientation.y,
    msg->pose.pose.orientation.z,
    msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
pose2d.theta = yaw;

There's a practical post about quaternion to yaw conversion. I think it can help you. http://www.theconstructsim.com/treati...

I've also created a Gist, because I always forget how to do it :) https://gist.github.com/marcoarruda/f...

Cheers!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-06-26 16:52:03 -0500

Seen: 16,634 times

Last updated: Aug 09 '17