Ask Your Question
0

Publishing Odometry in ROS2 - orientation is not reflected in RViz

asked 2020-03-15 01:02:30 -0500

Leonti gravatar image

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:

image description

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)
odom_broadcaster = tf2_ros.TransformBroadcaster(node, qos_profile_services_default)

x = 0.0
y = 0.0
th = 0.0
WHEEL_BASE = 0.177
while True:
  bytesToRead = ser.inWaiting()
  if bytesToRead > 0:

    line = str(ser.readline(), 'ascii')
    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.header.stamp = current_time
    t.header.frame_id = "odom"
    t.child_frame_id = "base_link"
    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_broadcaster.sendTransform(t)

    odom = Odometry()
    odom.header.frame_id = "odom"
    odom.header.stamp = current_time

    # 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.child_frame_id = "base_link"
    odom.twist.twist.linear.x = vx
    odom.twist.twist.linear.y = vy
    odom ...
(more)
edit retag flag offensive close merge delete

Comments

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.

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

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

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-03-17 06:49:06 -0500

Leonti gravatar image

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/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 :)

edit flag offensive delete link more

Comments

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.

gvdhoorn gravatar image gvdhoorn  ( 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 ;)

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-03-15 01:02:30 -0500

Seen: 1,802 times

Last updated: Mar 17 '20