Removed even more comments!

ros2/src/robobin/robobin/uwb_navigation_node.py

@@ -676,18 +676,29 @@ class UWBNode(Node):
         dy = calib_end_position[1] - self.calib_start_position[1]
         uwb_heading = math.atan2(dy, dx)
 
-        offset = calib_end_yaw - uwb_heading
-        self.uwb_to_imu_offset = offset
+        # Convert UWB heading to degrees
+        uwb_heading_deg = math.degrees(uwb_heading)
 
+        # Normalize to [0, 360)
+        uwb_heading_deg_normalized = (uwb_heading_deg + 360) % 360
 
-        self.get_logger().info("CALIBRATION COMPLETE!")
-        self.get_logger().info(f" Start Pos (UWB):   {self.calib_start_position}")
-        self.get_logger().info(f" End Pos (UWB):     {calib_end_position}")
-        self.get_logger().info(f" UWB heading:       {uwb_heading:.3f} rad")
-        self.get_logger().info(f" IMU yaw:           {calib_end_yaw:.3f} rad")
-        self.get_logger().info(f" --> OFFSET:        {offset:.3f} rad")
+        # Convert IMU yaw to degrees
+        calib_end_yaw_deg = math.degrees(calib_end_yaw)
 
-        
+        # Calculate offset
+        offset_deg = calib_end_yaw_deg - uwb_heading_deg_normalized
+
+        # Normalize offset to [0, 360)
+        offset_deg_normalized = (offset_deg + 360) % 360
+
+        # Logging information
+        print("CALIBRATION COMPLETE!")
+        print(f" Start Pos (UWB):   {self.calib_start_position}")
+        print(f" End Pos (UWB):     {calib_end_position}")
+        print(f" UWB heading:       {uwb_heading:.3f} rad ({uwb_heading_deg_normalized:.3f} deg)")
+        print(f" IMU yaw:           {calib_end_yaw:.3f} rad ({calib_end_yaw_deg:.3f} deg)")
+        print(f" --> OFFSET:        {offset_deg:.3f} deg (normalized to {offset_deg_normalized:.3f} deg)")
+                
         self.calibration_active = False
         self.mode = "Manual"