Ask Your Question
1

robot_pose_ekf wrong?

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

apalomer gravatar image

Hi,

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?

Thanks!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

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

Your Answer

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

Add Answer

Question Tools

Stats

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

Seen: 425 times

Last updated: Feb 03 '12