Last active
August 23, 2020 17:27
-
-
Save jones2126/f6060321024a827775bee3da0910ab38 to your computer and use it in GitHub Desktop.
Reads quaternion from Odometry and uses euler_from_quaternion function to print the yaw value in radians and degrees
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 | |
# 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