How to properly publish odometry data and subscribe to it

edit

Hi,

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

rospy.init_node('odometry_publisher')

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 = rospy.Time.now()
last_time = rospy.Time.now()

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

     # 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
     odom_broadcaster.sendTransform(
         (x, y, 0.),
         odom_quat,
         current_time,
         "base_footprint",
         "odom"
     )

     # 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
     odom_pub.publish(odom)

     last_time = current_time
     r.sleep()

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.

Thanks