From de28947d6fa892a52d4f778cb1d5b806c2665606 Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Wed, 8 Jan 2025 20:41:31 +0000
Subject: [PATCH] Removed even more comments!
---
.../robobin/robobin/uwb_navigation_node.py | 29 +++++++++++++------
1 file changed, 20 insertions(+), 9 deletions(-)
diff --git a/ros2/src/robobin/robobin/uwb_navigation_node.py b/ros2/src/robobin/robobin/uwb_navigation_node.py
index a091c8dc..5f1f80c1 100644
--- a/ros2/src/robobin/robobin/uwb_navigation_node.py
+++ b/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"
--
GitLab