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!
Originally posted by apalomer on ROS Answers with karma: 318 on 2012-02-03
Post score: 1
