From 5dd8b2c27b99d2170636e29e41a81a62539c0547 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Tue, 14 Jan 2025 09:30:25 +0100
Subject: [PATCH] update

---
 nodes/odom_publisher_python.py | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/nodes/odom_publisher_python.py b/nodes/odom_publisher_python.py
index 0438f6c..2d88f06 100644
--- a/nodes/odom_publisher_python.py
+++ b/nodes/odom_publisher_python.py
@@ -3,17 +3,27 @@
 import rospy
 from nav_msgs.msg import Odometry
 import tf
+import math
+
+def normalize_quaternion(x, y, z, w):
+    norm = math.sqrt(x**2 + y**2 + z**2 + w**2)
+    return x / norm, y / norm, z / norm, w / norm
 
 def odom_callback(msg):
     # Extract position and orientation from the odometry message
     position = msg.pose.pose.position
     orientation = msg.pose.pose.orientation
-    
+
+    # Normalize the quaternion
+    qx, qy, qz, qw = normalize_quaternion(
+        orientation.x, orientation.y, orientation.z, orientation.w
+    )
+
     # Broadcast the transformation
     br = tf.TransformBroadcaster()
     br.sendTransform(
         (position.x, position.y, position.z),  # Translation
-        (orientation.x, orientation.y, orientation.z, orientation.w),  # Rotation (quaternion)
+        (qx, qy, qz, qw),  # Normalized rotation (quaternion)
         rospy.Time.now(),  # Timestamp
         msg.child_frame_id if msg.child_frame_id else "base_link",  # Child frame ID
         msg.header.frame_id if msg.header.frame_id else "odom"  # Parent frame ID