Revision history [back]

Turns out the problem was this:

Below is the code, please note that I'm using quaternion_from_euler from transformations library

Even though these libraries cite the same author Christoph Gohlke the code for quaternion_from_euler is actually different.

Here is the code from the library that I was using:

try:
    firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
    _TUPLE2AXES[axes]  # validation
    firstaxis, parity, repetition, frame = axes

i = firstaxis + 1
j = _NEXT_AXIS[i+parity-1] + 1
k = _NEXT_AXIS[i-parity] + 1

if frame:
    ai, ak = ak, ai
if parity:
    aj = -aj

ai /= 2.0
aj /= 2.0
ak /= 2.0
ci = math.cos(ai)
si = math.sin(ai)
cj = math.cos(aj)
sj = math.sin(aj)
ck = math.cos(ak)
sk = math.sin(ak)
cc = ci*ck
cs = ci*sk
sc = si*ck
ss = si*sk

q = numpy.empty((4, ))
if repetition:
    q[0] = cj*(cc - ss)
    q[i] = cj*(cs + sc)
    q[j] = sj*(cc + ss)
    q[k] = sj*(cs - sc)
else:
    q[0] = cj*cc + sj*ss
    q[i] = cj*sc - sj*cs
    q[j] = cj*ss + sj*cc
    q[k] = cj*cs - sj*sc
if parity:
    q[j] *= -1.0

return q

https://github.com/malcolmreynolds/transformations/blob/master/transformations/transformations.py#L1174

And this is the one packaged with ROS1:

try:
    firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
    _ = _TUPLE2AXES[axes]
    firstaxis, parity, repetition, frame = axes

i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]

if frame:
    ai, ak = ak, ai
if parity:
    aj = -aj

ai /= 2.0
aj /= 2.0
ak /= 2.0
ci = math.cos(ai)
si = math.sin(ai)
cj = math.cos(aj)
sj = math.sin(aj)
ck = math.cos(ak)
sk = math.sin(ak)
cc = ci*ck
cs = ci*sk
sc = si*ck
ss = si*sk

quaternion = numpy.empty((4, ), dtype=numpy.float64)
if repetition:
    quaternion[i] = cj*(cs + sc)
    quaternion[j] = sj*(cc + ss)
    quaternion[k] = sj*(cs - sc)
    quaternion[3] = cj*(cc - ss)
else:
    quaternion[i] = cj*sc - sj*cs
    quaternion[j] = cj*ss + sj*cc
    quaternion[k] = cj*cs - sj*sc
    quaternion[3] = cj*cc + sj*ss
if parity:
    quaternion[j] *= -1

return quaternion

https://github.com/ros/geometry/blob/hydro-devel/tf/src/tf/transformations.py#L1100

I just copied transformations.py into my project and now odometry works as expected :)