From 211cb799f7f37d526b779aff5249fadc640d57a9 Mon Sep 17 00:00:00 2001
From: jason <jasonah@seas.upenn.edu>
Date: Fri, 17 Oct 2025 16:29:17 -0400
Subject: [PATCH] added test script for differential gps

---
 glider/scripts/odometry_heading.py | 49 ++++++++++++++++++++++++++++++
 1 file changed, 49 insertions(+)
 create mode 100644 glider/scripts/odometry_heading.py

diff --git a/glider/scripts/odometry_heading.py b/glider/scripts/odometry_heading.py
new file mode 100644
index 0000000..06dade8
--- /dev/null
+++ b/glider/scripts/odometry_heading.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from scipy.spatial.transform import Rotation
+
+class OdomHeadingSubscriber(Node):
+    def __init__(self):
+        super().__init__('odom_heading_subscriber')
+        self.subscription = self.create_subscription(
+            Odometry,
+            '/glider/odom',
+            self.odom_callback,
+            10)
+        self.get_logger().info('Odometry heading subscriber initialized')
+
+    def odom_callback(self, msg):
+        # Extract quaternion from odometry message
+        orientation = msg.pose.pose.orientation
+        quat = [orientation.x, orientation.y, orientation.z, orientation.w]
+        
+        # Convert quaternion to Euler angles using scipy
+        rotation = Rotation.from_quat(quat)
+        euler = rotation.as_euler('xyz', degrees=False)
+        
+        # Extract yaw (rotation around z-axis)
+        yaw = euler[2]
+        
+        # Convert radians to degrees
+        heading_degrees = yaw * 180.0 / 3.141592653589793
+        
+        # Print the heading
+        self.get_logger().info(f'Heading: {heading_degrees:.2f} degrees')
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomHeadingSubscriber()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()