ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

robot_pose_ekf wrong?

asked 2012-02-03 02:13:36 -0500

apalomer gravatar image


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?


edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2012-02-03 02:27:27 -0500

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

edit flag offensive delete link more

Question Tools


Asked: 2012-02-03 02:13:36 -0500

Seen: 546 times

Last updated: Feb 03 '12