why my turtlebot is not moving with odometry info?

asked 2021-10-28 09:19:45 -0500

MPR gravatar image

updated 2022-03-08 07:00:00 -0500

lucasw gravatar image

i was using the below code.This doesnot move my turtlebot.What could be the reason?

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

last_time = current_time
r.sleep()
edit retag flag offensive close merge delete

Comments

Please take a look at this question, similar issue: https://answers.ros.org/question/2416...

osilva gravatar image osilva  ( 2021-10-28 09:29:31 -0500 )edit

could you help me with some python code?

MPR gravatar image MPR  ( 2021-10-28 09:32:41 -0500 )edit

Take a look at this discussion: https://gist.github.com/atotto/f2754f... Hope this helps. It's mainly in Python.

osilva gravatar image osilva  ( 2021-10-28 09:36:50 -0500 )edit