Possible bug in HDT heading to Quat Conversions

[jediofgever]

Hi,
I think there is a bug the way the driver converts heading to quats.
My GPS provides $GPHDT so the driver publishes a topic /heading
I tracked down an error in my system and realized that the x component coming from /heading is non-zero, where as x and y should report zeros.

The issue is, lack of attention given to return format of quaternion_from_euler function call.

The function footprint is as follows;

(function) quaternion_from_euler: (ai, aj, ak, axes='sxyz') -> ndarray
Return quaternion from Euler angles and axis sequence axes

Parameters
ai : float
    First rotation angle (according to axes).
aj : float
    Second rotation angle (according to axes).
ak : float
    Third rotation angle (according to axes).
axes : str, optional
    Axis specification; one of 24 axis sequences as string or encoded tuple - e.g. sxyz (the default).

Returns
quat : array shape (4,)
   Quaternion in w, x, y z (real, then vector) format

The first component of returned array is w but the driver assumes it is x

  q = quaternion_from_euler(0, 0, math.radians(data['heading']))
  current_heading.quaternion.x = q[0]
  current_heading.quaternion.y = q[1]
  current_heading.quaternion.z = q[2]
  current_heading.quaternion.w = q[3]

This is a critical issue and needs to be addressed quickly.

Edit,

Opened a PR for ros2 branch here #134

[evenator]

This problem stems from an API incompatibility issue between the ROS1 tf.transformations package and the Python transformations3d package, which is summarized nicely on the README of the ROS2 tf_transformations package.

In short, the ROS1 version of quaternion_from_euler returned a tuple with w as the last component, but transformations3d returns a tuple with w as the first component.
The fix proposed here appears to be correct.

[evenator]

Fixed by #134