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 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 zeroes
Originally posted by Leonti on ROS Answers with karma: 62 on 2020-03-15
Post score: 0
Original comments
Comment by gvdhoorn on 2020-03-15:\
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.
Comment by Leonti on 2020-03-17:
Thanks for the hint! This made me look into quaternions which led to a "solution" ;)