How to properly publish odometry data and subscribe to it

asked 2018-04-16 17:02:28 -0500

startimeahmet gravatar image

updated 2018-04-21 13:30:26 -0500


I am trying to get the odometry data from my robot's wheel encoders for SLAM purposes. I have tried this code for publishing the data to /odom :

#!/usr/bin/env python

import math
from math import sin, cos, pi
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3


odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()

x = 0.0
y = 0.0
th = 0.0

vx = 0.1
vy = -0.1
vth = 0.1

current_time =
last_time =

r = rospy.Rate(1.0)
while not rospy.is_shutdown():
     current_time =

     # compute odometry in a typical way given the velocities of the robot
     dt = (current_time - last_time).to_sec()
     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

     # since all odometry is 6DOF we'll need a quaternion created from yaw
     odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

     # first, we'll publish the transform over tf
         (x, y, 0.),

     # next, we'll publish the odometry message over ROS
     odom = Odometry()
     odom.header.stamp = current_time
     odom.header.frame_id = "odom"

     # set the position
     odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

     # set the velocity
     odom.child_frame_id = "base_footprint"
     odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

     # publish the message

     last_time = current_time

This seems to work however when I try to create a 2D map following the steps in this tutorial (rtabmap), I am having this problem:

[ WARN] [1523960272.165185345]: rtabmap: Could not get transform from odom to base_footprint after 0.200000 seconds (for stamp=1523960271.944494)!
[ WARN] [1523960272.383778992]: rtabmap: Could not get transform from odom to base_footprint after 0.200000 seconds (for stamp=1523960272.144028)!
[ WARN] [1523960272.601640937]: rtabmap: Could not get transform from odom to base_footprint after 0.200000 seconds (for stamp=1523960272.378175)!

When I run the command, rostopic echo /odom, I get the data. But somehow it is not transferred to the Rtabmap side.

Any help on this would be highly appreciated.


edit retag flag offensive close merge delete


Maybe more information could help. Such as tf tree and node graph

Humpelstilzchen gravatar image Humpelstilzchen  ( 2018-04-17 01:24:56 -0500 )edit

I am not sure how to get the tf tree. For the node graph, when I run rqt_graph, I get some errors and nothing shows up at the opened window.

startimeahmet gravatar image startimeahmet  ( 2018-04-17 05:19:29 -0500 )edit

To get the tf tree, you can use rosrun tf view_frames and this will generate a pdf file called frames.pdf and containing the transforms tree. Or you can simply visualize it with rqt and take a screen shot. see this link for more details about tf monitoring.

Jasmin gravatar image Jasmin  ( 2018-04-17 15:27:44 -0500 )edit

Tf tree Here is the tf tree. Since it was too long horizontally, i took the middle portion of it.

startimeahmet gravatar image startimeahmet  ( 2018-04-21 13:27:15 -0500 )edit

Not sure if thats the cause of your problems, but from the error you can see that rtabmap waits a maximum time of 0.2 seconds for a transform while in the tf tree the average rate is only 1Hz.

Humpelstilzchen gravatar image Humpelstilzchen  ( 2018-04-21 13:59:46 -0500 )edit