From f187fab3e9d554181369b9df57cc7f64610e4254 Mon Sep 17 00:00:00 2001
From: Saransh Agrawal <saransh323@gmail.com>
Date: Wed, 6 Nov 2024 16:42:10 -0500
Subject: [PATCH] changed quaternion to yaw conversion

---
 python-requirements.txt                        |  1 +
 rb_ws/src/buggy/buggy/buggy_state_converter.py | 17 ++++++++++++++---
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/python-requirements.txt b/python-requirements.txt
index 71c2c61..12d4591 100644
--- a/python-requirements.txt
+++ b/python-requirements.txt
@@ -9,6 +9,7 @@ pyproj
 rclpy
 scipy
 setuptools==58.2.0
+tf_transformations
 trimesh
 utm
 keyboard
diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py
index f3dd023..4fc2f1d 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
 
 class BuggyStateConverter(Node):
     def __init__(self):
@@ -70,9 +70,19 @@ def convert_SC_state(self, msg):
         converted_msg.pose.pose.position.z = 0.0    # ignored
 
         # ---- 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
-        yaw = self.quaternion_to_yaw(qx, qy, qz, qw)
-        converted_msg.pose.pose.orientation.x = yaw
+
+        # replaced with tf_transformations library function
+        # yaw = self.quaternion_to_yaw(qx, qy, qz, qw)
+        # converted_msg.pose.pose.orientation.x = yaw
+
+        # Use euler_from_quaternion to get roll, pitch, yaw
+        _, _, yaw = euler_from_quaternion([qx, qy, qz, qw])
+        yaw_aligned_east = yaw - np.pi / 2   # TODO: check if 0 is East
+        # Store the heading in the x component of the orientation
+        converted_msg.pose.pose.orientation.x = yaw_aligned_east
+
         # ignored:
         converted_msg.pose.pose.orientation.y = 0.0
         converted_msg.pose.pose.orientation.z = 0.0
@@ -130,6 +140,7 @@ def convert_NAND_state(self, msg):
 
         return converted_msg
 
+    # replaced custom function with tf_transformations library function
     def quaternion_to_yaw(self, qx, qy, qz, qw):
         """Convert a quaternion to yaw (heading) in radians."""
         siny_cosp = 2.0 * (qw * qz + qx * qy)