error reporting Euler angles

[jonazpiazu]

47 introduced a bug in how the Euler angles are reported.

As initial context: static_transform_publisher expects the angles to be specified in yaw pitch roll order (or ZYX), while URDF uses rpy order (or XYZ).

But the output as reported by #47 does not correspond to rpy.

As a counterexample, using the same numbers as used in this comment from the PR:

>>> from tf import transformations
>>> q=[-0.0372638, -0.0326999, 0.773564, 0.631776]
>>> transformations.euler_from_quaternion(q, axes='sxyz')
(-0.09784475354668137, 0.016334569880032665, 1.7711006453446172)

>>> transformations.euler_from_quaternion(q, axes='rxyz')
(0.0035234877615449344, -0.09913217272764203, 1.7720752160631836)

You can see that the orientation reported corresponds to axes='rxyz'.

So to properly report rpy the code should be something like:

    r_euler = calib_T.rotation().eulerAngles(2,1,0);
    std::cout << "rpy=" << r_euler[2] << "," << r_euler[1] << "," << r_euler[0] << std::endl;

I can prepare a proper PR for that if needed.

[captain-yoshi]

Same problem with my setup.

Calibration output:

<launch>
  <!-- The rpy in the comment uses the extrinsic XYZ convention, which is the same as is used in a URDF. See
       http://wiki.ros.org/geometry2/RotationMethods and https://en.wikipedia.org/wiki/Euler_angles for more info. -->
  <!-- xyz="-0.0852237 -0.0776651 0.0608126" rpy="2.97005 -2.35605 3.01759" -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="camera_link_broadcaster"
      args="-0.0852237 -0.0776651 0.0608126   -0.0553599 -0.385516 -0.0243088 0.920718 ur3_wrist_3_link camera_color_optical_frame" />
</launch>

Expected URDF RPY:

<!-- xyz="-0.0852237 -0.0776651 0.0608126" rpy="-0.1188682 -0.7931895 -0.0029631" -->