[fabianhirmann]

Description

• fix inconsistent results of tf2::getEulerYPR and tf2::getYaw compared to tf2::Matrix3x3::getEulerYPR by simply using the seemingly more stable version of tf2::Matrix3x3::getEulerYPR
  • More stable results in this case means that for the special case of (roll=0°, pitch=90°, yaw=0°) converted to a quaternion, tf2::Matrix3x3::getEulerYPR returns the expected result (roll=0°, pitch=90°, yaw=0°) (the same as the original input), while tf2::getEulerYPR and tf2::getYaw return (roll=0°, pitch=45°, yaw=180°).
• extend test cases for util functions (mostly taken from Fix numerical precision issues in quaternion to RPY conversion #811 but extended with cases that are still failing at that other PR)

Assuming the test cases from #811 are representing the issue in #796, this also fixes #796.

Is this user-facing behavior change?

Yes, before tf2::getEulerYPR and tf2::getYaw could produce different results than tf2::Matrix3x3::getEulerYPR (although stated in the comment of tf2::getEulerYPR that the results should be the same). Now with this change since simply tf2::Matrix3x3::getEulerYPR is reused, the results are exactly the same.

Did you use Generative AI?

Additional Information

[fabianhirmann]

Since there is no API/ABI change, please backport this change to at least humble as well.