Forked from atotto/ros_odometry_publisher_example.py
Created
December 26, 2019 02:45
-
-
Save jdrew1303/66d7f7ad72a6befa3697ca288af6fef6 to your computer and use it in GitHub Desktop.
Publishing Odometry Information over ROS (python)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment