simple fake odometry publisher [closed]

asked 2019-10-12 04:31:24 -0500

june2473 gravatar image

updated 2019-10-12 23:37:08 -0500

I want to test rtabmap. I have d435 camera, but I dont have wheels yet.

So, I want to use simple fake odometry publisher, to publish zeros, to pretend that robot is not moving.

I tried this code, but I am not sure that it is working correctly, because I have got freezes of mapCloud. And I am afraid it is somehow related to this code.

#!/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.0
vy = 0.0
vth = 0.0

current_time =
last_time =

r = rospy.Rate(50.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
  odom_broadcaster.sendTransform( (x, y, 0.), odom_quat, current_time, "base_link",  "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_link"
  odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

  # publish the message

  last_time = current_time

Is this code correct to publish zeros in odometry?

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by june2473
close date 2019-12-02 23:38:44.762420