"tf2::Matrix3x3::getRPY" and "tf2::Matrix3x3::getEulerYPR" function is wrong.

[nyxrobotics]

The Euler angle changes in the order of calculation. However, both getEulerYPR and getRPY defined in TF2 :: Matrix3x3 incorrectly use the order (that is YRP).

[torydebra]

Any news about this? Also for me it results that tf2 :: Matrix3x3::getRPY is wrong:

tf2::Matrix3x3 mat(0,  0, 1,
                   0, -1, 0,
                   1,  0, 0);

tf2::Vector3 rpy;
mat.getRPY(rpy[0], rpy[1], rpy[2]);

tf2::Matrix3x3 mat2;
mat2.setRPY(rpy[0], rpy[1], rpy[2]);

std::cout << "mat" << std::endl;
std::cout << mat.getRow(0)[0] << " " << mat.getRow(0)[1] << " " << mat.getRow(0)[2] << std::endl;
std::cout << mat.getRow(1)[0] << " " << mat.getRow(1)[1] << " " << mat.getRow(1)[2] << std::endl;
std::cout << mat.getRow(2)[0] << " " << mat.getRow(2)[1] << " " << mat.getRow(2)[2] << std::endl;
std::cout << std::endl;
std::cout << "rpy" << std::endl;
std::cout << rpy[0] << std::endl;
std::cout << rpy[1] << std::endl;
std::cout << rpy[2] << std::endl;
std::cout << std::endl;
std::cout << "mat2" << std::endl;
std::cout << mat2.getRow(0)[0] << " " << mat2.getRow(0)[1] << " " << mat2.getRow(0)[2] << std::endl;
std::cout << mat2.getRow(1)[0] << " " << mat2.getRow(1)[1] << " " << mat2.getRow(1)[2] << std::endl;
std::cout << mat2.getRow(2)[0] << " " << mat2.getRow(2)[1] << " " << mat2.getRow(2)[2] << std::endl;
std::cout << std::endl;

prints out:

mat
0 0 1
0 -1 0
1 0 0

rpy
0
-1.5708
0

mat2
6.12323e-17 -0 -1
0 1 -0
1 0 6.12323e-17