Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Reads quaternion from Odometry and uses euler_from_quaternion function to print the yaw value in radians and degrees
#!/usr/bin/env python
# check_yaw_from_odom.py
# Running Turtlebot in simulation yaw ranges from 3.14 to -3.14
# which converts in degrees to 180 degrees and -180 degrees respectively
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler
print("starting check_yaw_from_odom")
def get_rotation (msg):
global roll, pitch, yaw
orientation_q = msg.pose.pose.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
degree = (yaw*180)/3.1416
print('Yaw - radian: %.3f degree: %.1f' % (yaw, degree))
rospy.init_node('check_yaw_from_odom')
sub = rospy.Subscriber ('/odom', Odometry, get_rotation)
r = rospy.Rate(1)
while not rospy.is_shutdown():
r.sleep()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment