ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
I think you are looking for the setEulerYPR
and getEulerYPR
functions of tf::Matrix3x3
. As far as I can tell, these follow the convention you are describing.
To generate the quaternion:
//Fixed transform values
double roll, pitch, yaw;
yaw = DEGREES_TO_RADIANS(90); // 90 degrees around Z
pitch = DEGREES_TO_RADIANS(0); // 0 degrees around new Y
roll = DEGREES_TO_RADIANS(-90); // -90 degrees around new X
//Create Matrix3x3 from Euler Angles
tf::Matrix3x3 m_rot;
m_rot.setEulerYPR(yaw, pitch, roll);
// Convert into quaternion
tf::Quaternion quat;
m_rot.getRotation(quat);
Your printEuler would then be
void printEulerAngles(tf::StampedTransform& transform){
double roll, pitch, yaw;
tf::Matrix3x3 m(transform.getRotation());
int solution_number = 1;
m.getEulerYPR(yaw, pitch, roll, solution_number);
ROS_INFO("\nRoll : %.3f\n"
"Pitch : %.3f\n"
"Yaw : %.3f\n",
RADIANS_TO_DEGREES(roll), RADIANS_TO_DEGREES(pitch), RADIANS_TO_DEGREES(yaw) );
}
However, like I wrote in your previous question, there can be multiple representations of an identical rotation, so your angles might not be the same. You can play around witht the solution_number
argument of getEulerYPR()
but I don't know if that will help.
Good luck.