From 86da93ce7d30b012e5861086ef337139fa2ecf4b Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 4 Dec 2024 16:54:55 -0500 Subject: [PATCH] ditch tf_transformations for scipy rotation --- rb_ws/src/buggy/buggy/buggy_state_converter.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 7a73053..bd21bea 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -5,7 +5,7 @@ from nav_msgs.msg import Odometry import numpy as np import pyproj -from tf_transformations import euler_from_quaternion +from scipy.spatial.transform import Rotation class BuggyStateConverter(Node): def __init__(self): @@ -78,8 +78,9 @@ def convert_SC_state(self, msg): # ---- 2. Convert Quaternion to Heading (Radians) ---- qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w - # Use euler_from_quaternion to get roll, pitch, yaw - roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) + # Use Rotation.from_quat to get roll, pitch, yaw + roll, pitch, yaw = Rotation.from_quat([qx, qy, qz, qw]).as_euler('xyz'); + # roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) # tf_transformations bad # Store the heading in the x component of the orientation converted_msg.pose.pose.orientation.x = roll