ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Tue, 17 Mar 2020 07:22:47 -0500Publishing Odometry in ROS2 - orientation is not reflected in RVizhttps://answers.ros.org/question/346742/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:
![image description](/upfiles/15842519887434533.png)
Below is the code, please note that I'm using `quaternion_from_euler` from `transformations` library https://pypi.org/project/transformations/, 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.twist.twist.angular.z = vth
odom_pub.publish(odom)
time.sleep(.001)
Here is an example of the `Odometry message`:
header:
stamp:
sec: 1584244609
nanosec: 895687741
frame_id: odom
child_frame_id: base_link
pose:
pose:
position:
x: 0.6140683483040018
y: -2.3255323685351637
z: 0.0
orientation:
x: -0.36651838967766565
y: 0.0
z: -0.0
w: -0.9304108071320383
covariance: - all zeroes
twist:
twist:
linear:
x: 0.08174239999999999
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.5080033898305085
covariance: - all zeroesSun, 15 Mar 2020 01:02:30 -0500https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/Comment by Leonti for <div class="snippet"><p>Hi! <br>
I'm trying to publish odometry messages based on encoder values coming from motors. <br>
I'm using ROS2 (Eloquent). <br>
Odometry messages are published, but the orientation fo the robot is not correct (the arrow is always pointing up in RViz) </p>
<p>Below are more details. <br>
Motors are controlled by Arduino which uses Serial port. <br>
I send desired velocities in mm/s (linear) and radians/s (angular). <br>
Arduno sends back actual speeds for left and right wheels based on encoder data as comma-separated lines: </p>
<pre><code>duration_ms,left_velocity_mm_s,right_velocity_mm_s
</code></pre>
<p>After calculating linear and angular velocities they are very similar to the desired ones. <br>
For example, when I send <code>0.08</code> m/s linear and <code>0.5</code> r/s angular <code>vx</code> and <code>vth</code> are something like <code>0.086</code> and <code>0.023091</code> when going straingt and
<code>left: 0.041, right: 0.119 vth: 0.439</code> when turning left.</p>
<p>The problem is that after launching RViz2 and adding <code>Odometry</code> 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. <br>
It looks like the robot is moving sideways which it can't do :) <br>
As far as I understand it should point in the direction in which robot is pointed. </p>
<p>What am I missing here?</p>
<p>Here is how it looks in RViz:</p>
<p><img src="/upfiles/15842519887434533.png" alt="image description"></p>
<p>Below is the code, please note that I'm using <code>quaternion_from_euler</code> from <code>transformations</code>library <a href="https://pypi.org/project/transformations/">https://pypi.org/project/transformati...</a>, because I think it's not packaged with <code>ros2_tf</code>.</p>
<pre><code>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 ...</code></pre><span class="expander"> <a>(more)</a></span></div>https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346888#post-id-346888Thanks for the hint! This made me look into quaternions which led to a "solution" ;)Tue, 17 Mar 2020 07:22:47 -0500https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346888#post-id-346888Comment by gvdhoorn for <div class="snippet"><p>Hi! <br>
I'm trying to publish odometry messages based on encoder values coming from motors. <br>
I'm using ROS2 (Eloquent). <br>
Odometry messages are published, but the orientation fo the robot is not correct (the arrow is always pointing up in RViz) </p>
<p>Below are more details. <br>
Motors are controlled by Arduino which uses Serial port. <br>
I send desired velocities in mm/s (linear) and radians/s (angular). <br>
Arduno sends back actual speeds for left and right wheels based on encoder data as comma-separated lines: </p>
<pre><code>duration_ms,left_velocity_mm_s,right_velocity_mm_s
</code></pre>
<p>After calculating linear and angular velocities they are very similar to the desired ones. <br>
For example, when I send <code>0.08</code> m/s linear and <code>0.5</code> r/s angular <code>vx</code> and <code>vth</code> are something like <code>0.086</code> and <code>0.023091</code> when going straingt and
<code>left: 0.041, right: 0.119 vth: 0.439</code> when turning left.</p>
<p>The problem is that after launching RViz2 and adding <code>Odometry</code> 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. <br>
It looks like the robot is moving sideways which it can't do :) <br>
As far as I understand it should point in the direction in which robot is pointed. </p>
<p>What am I missing here?</p>
<p>Here is how it looks in RViz:</p>
<p><img src="/upfiles/15842519887434533.png" alt="image description"></p>
<p>Below is the code, please note that I'm using <code>quaternion_from_euler</code> from <code>transformations</code>library <a href="https://pypi.org/project/transformations/">https://pypi.org/project/transformati...</a>, because I think it's not packaged with <code>ros2_tf</code>.</p>
<pre><code>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 ...</code></pre><span class="expander"> <a>(more)</a></span></div>https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346750#post-id-346750> 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](https://quaternions.online/), I only get `0`s, which would correspond to what you are describing.
I'd check whether your calculations are correct.Sun, 15 Mar 2020 05:34:18 -0500https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346750#post-id-346750Answer by Leonti for <div class="snippet"><p>Hi! <br>
I'm trying to publish odometry messages based on encoder values coming from motors. <br>
I'm using ROS2 (Eloquent). <br>
Odometry messages are published, but the orientation fo the robot is not correct (the arrow is always pointing up in RViz) </p>
<p>Below are more details. <br>
Motors are controlled by Arduino which uses Serial port. <br>
I send desired velocities in mm/s (linear) and radians/s (angular). <br>
Arduno sends back actual speeds for left and right wheels based on encoder data as comma-separated lines: </p>
<pre><code>duration_ms,left_velocity_mm_s,right_velocity_mm_s
</code></pre>
<p>After calculating linear and angular velocities they are very similar to the desired ones. <br>
For example, when I send <code>0.08</code> m/s linear and <code>0.5</code> r/s angular <code>vx</code> and <code>vth</code> are something like <code>0.086</code> and <code>0.023091</code> when going straingt and
<code>left: 0.041, right: 0.119 vth: 0.439</code> when turning left.</p>
<p>The problem is that after launching RViz2 and adding <code>Odometry</code> 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. <br>
It looks like the robot is moving sideways which it can't do :) <br>
As far as I understand it should point in the direction in which robot is pointed. </p>
<p>What am I missing here?</p>
<p>Here is how it looks in RViz:</p>
<p><img src="/upfiles/15842519887434533.png" alt="image description"></p>
<p>Below is the code, please note that I'm using <code>quaternion_from_euler</code> from <code>transformations</code>library <a href="https://pypi.org/project/transformations/">https://pypi.org/project/transformati...</a>, because I think it's not packaged with <code>ros2_tf</code>.</p>
<pre><code>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 ...</code></pre><span class="expander"> <a>(more)</a></span></div> https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?answer=346882#post-id-346882Turns 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 :)
Tue, 17 Mar 2020 06:49:06 -0500https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?answer=346882#post-id-346882Comment by Leonti for <p>Turns out the problem was this:</p>
<blockquote>
<p>Below is the code, please note that I'm using quaternion_from_euler from transformations library </p>
</blockquote>
<p>Even though these libraries cite the same author <code>Christoph Gohlke</code> the code for <code>quaternion_from_euler</code> is actually different. </p>
<p>Here is the code from the library that I was using: </p>
<pre><code>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
</code></pre>
<p><a href="https://github.com/malcolmreynolds/transformations/blob/master/transformations/transformations.py#L1174">https://github.com/malcolmreynolds/tr...</a></p>
<p>And this is the one packaged with ROS1:</p>
<pre><code>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
</code></pre>
<p><a href="https://github.com/ros/geometry/blob/hydro-devel/tf/src/tf/transformations.py#L1100">https://github.com/ros/geometry/blob/...</a></p>
<p>I just copied <code>transformations.py</code> into my project and now odometry works as expected :) </p>
https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346887#post-id-346887My 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 ;)Tue, 17 Mar 2020 07:22:06 -0500https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346887#post-id-346887Comment by gvdhoorn for <p>Turns out the problem was this:</p>
<blockquote>
<p>Below is the code, please note that I'm using quaternion_from_euler from transformations library </p>
</blockquote>
<p>Even though these libraries cite the same author <code>Christoph Gohlke</code> the code for <code>quaternion_from_euler</code> is actually different. </p>
<p>Here is the code from the library that I was using: </p>
<pre><code>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
</code></pre>
<p><a href="https://github.com/malcolmreynolds/transformations/blob/master/transformations/transformations.py#L1174">https://github.com/malcolmreynolds/tr...</a></p>
<p>And this is the one packaged with ROS1:</p>
<pre><code>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
</code></pre>
<p><a href="https://github.com/ros/geometry/blob/hydro-devel/tf/src/tf/transformations.py#L1100">https://github.com/ros/geometry/blob/...</a></p>
<p>I just copied <code>transformations.py</code> into my project and now odometry works as expected :) </p>
https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346883#post-id-346883Seeing 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.Tue, 17 Mar 2020 06:57:02 -0500https://answers.ros.org/question/346742/publishing-odometry-in-ros2-orientation-is-not-reflected-in-rviz/?comment=346883#post-id-346883