# Publishing Odometry in ROS2 - orientation is not reflected in RViz

Hi!
I'm trying to publish odometry messages based on encoder values coming from motors.
I'm using ROS2 (Eloquent).
Odometry messages are published, but the orientation fo the robot is not correct (the arrow is always pointing up in RViz)

Below are more details.
Motors are controlled by Arduino which uses Serial port.
I send desired velocities in mm/s (linear) and radians/s (angular).
Arduno sends back actual speeds for left and right wheels based on encoder data as comma-separated lines:

duration_ms,left_velocity_mm_s,right_velocity_mm_s


After calculating linear and angular velocities they are very similar to the desired ones.
For example, when I send 0.08 m/s linear and 0.5 r/s angular vx and vth are something like 0.086 and 0.023091 when going straingt and left: 0.041, right: 0.119 vth: 0.439 when turning left.

The problem is that after launching RViz2 and adding Odometry display I can see that the position of the robot is changing and it follows my commands, but the direction is always the same - it's always pointing up.
It looks like the robot is moving sideways which it can't do :)
As far as I understand it should point in the direction in which robot is pointed.

What am I missing here?

Here is how it looks in RViz:

Below is the code, please note that I'm using quaternion_from_euler from transformationslibrary https://pypi.org/project/transformati..., because I think it's not packaged with ros2_tf.

odom_pub = node.create_publisher(
Odometry, '/odom', qos_profile=qos_profile_services_default)

x = 0.0
y = 0.0
th = 0.0
WHEEL_BASE = 0.177
while True:

parts = line.split(",")

dt = int(parts[0]) / 1000.0 # convert to seconds
v_left = float(parts[1]) / 1000.0 # convert to metres from mm
v_right = float(parts[2]) / 1000.0
vx = (v_right + v_left) / 2.0
vy = 0.0
vth = (v_right - v_left) / WHEEL_BASE

delta_x = (vx * cos(th) - vy * sin(th)) * dt
delta_y = (vx * sin(th) + vy * cos(th)) * dt
delta_th = vth * dt

x += delta_x
y += delta_y
th += delta_th

odom_quat = quaternion_from_euler(0, 0, th)
current_time = node.get_clock().now().to_msg()

t = TransformStamped()
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = 0.0
t.transform.rotation.x = odom_quat[0]
t.transform.rotation.y = odom_quat[1]
t.transform.rotation.z = odom_quat[2]
t.transform.rotation.w = odom_quat[3]

odom = Odometry()

# set the position
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.x = odom_quat[0]
odom.pose.pose.orientation.y = odom_quat[1]
odom.pose.pose.orientation.z = odom_quat[2]
odom.pose.pose.orientation.w = odom_quat[3]

# set the velocity
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom ...
edit retag close merge delete

orientation:
x: -0.36651838967766565
y: 0.0
z: -0.0
w: -0.9304108071320383


without looking at anything else, this looks fishy.

If I put those values into this site, I only get 0s, which would correspond to what you are describing.

I'd check whether your calculations are correct.

( 2020-03-15 05:34:18 -0500 )edit

Thanks for the hint! This made me look into quaternions which led to a "solution" ;)

( 2020-03-17 07:22:47 -0500 )edit

Sort by » oldest newest most voted

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/tr...

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/...

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

more

Seeing as "quaternion from Euler angles" is essentially a mathematical transformation which is either correct or not, it seems strange for one version to work and the other not to work, unless one of them is incorrect.

It would be good to know which one, or what the actual problem is.

Have you considered notifying / asking the author?

The biggest difference in the code above seems to be the ordering of elements in the quaternion, and the explicit declaration of the data type.

( 2020-03-17 06:57:02 -0500 )edit

My bet would be as you said that it has to do with the ordering - at some point I almost got it working by switching the order of parameters supplied to quaternion_from_euler, it was quaternion_from_euler(th, 0, 0) instead of quaternion_from_euler(0, 0, th). It almost "worked" in a sense that the arrow started to point in correct directions, but the transform to "base_link" was upside-down. I'm sure if I new how to shuffle either parameters or x,y,z,w from the output I could get it working with the original library, but I don't have enough knowledge about quaternions to do that, so I did what I could - just replaced the library ;)

( 2020-03-17 07:22:06 -0500 )edit