Rosanswers logo


I'm trying to add a xsense to my robot. I've checked that the sensor is giving me correct orientation by transforming the quaternion to roll, pitch and yaw and checking that every angle turns arround it's axe.

After that I've done the same (this time creating a new node that subscribe to robot_pose_ekf/odom and prints in the screen RPY) with robot pose ekf, but when I turn my robot on the spot it says that it's turning Roll

I'm useing this code for tranforming from quaternion to RPY:

double roll, pitch, yaw;
roll = atan2(2*(msg->pose.pose.orientation.x*msg->pose.pose.orientation.y + msg->pose.pose.orientation.z*msg->pose.pose.orientation.w),1-2*(msg->pose.pose.orientation.y*msg->pose.pose.orientation.y+msg->pose.pose.orientation.z*msg->pose.pose.orientation.z)) * 180/PI;
pitch = asin(2*(msg->pose.pose.orientation.x* msg->pose.pose.orientation.z - msg->pose.pose.orientation.y*msg->pose.pose.orientation.w)) * 180/PI;
yaw = atan2(2*(msg->pose.pose.orientation.x*msg->pose.pose.orientation.w+msg->pose.pose.orientation.y*msg->pose.pose.orientation.z),1-2*(msg->pose.pose.orientation.z*msg->pose.pose.orientation.z+msg->pose.pose.orientation.w*msg->pose.pose.orientation.w)) * 180/PI;
ROS_INFO("RPY: (%f,%f,%f)",roll,pitch,yaw);

Do I have something wrong on the code or is the robot_pose_ekf publishin a bad orientation?


Originally posted by apalomer on ROS Answers with karma: 318 on 2012-02-03

Post score: 1


1 Answer 1


Rosanswers logo

This sounds like it might be related to mixing up North-East-Down and East-North-Up coordinate systems, as explained in this answer.

Originally posted by Stefan Kohlbrecher with karma: 24361 on 2012-02-03

This answer was ACCEPTED on the original site

Post score: 2


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.