Removed even more comments!

ros2/src/robobin/robobin/uwb_navigation_node.py

@@ -304,15 +304,15 @@ class UWBNode(Node):
             self.get_logger().warning("Insufficient beacons for trilateration.")
             return None
 
-        self.get_logger().info(f"Available beacons: {available_beacons}")
-        self.get_logger().info(f"Available distances: {available_distances}")
+        # self.get_logger().info(f"Available beacons: {available_beacons}")
+        # self.get_logger().info(f"Available distances: {available_distances}")
 
         heights = []
         for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
             horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2)
             estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2))
             heights.append(bz - estimated_z)
-        self.get_logger().info(f"Estimated heights: {heights}")
+        # self.get_logger().info(f"Estimated heights: {heights}")
 
         initial_z = max(0, min(self.anchorHeight, np.mean(heights)))
 
@@ -328,8 +328,8 @@ class UWBNode(Node):
         z_max = self.anchorHeight
         bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max])
 
-        self.get_logger().info(f"Initial guess: {initial_guess}")
-        self.get_logger().info(f"Bounds: {bounds}")
+        # self.get_logger().info(f"Initial guess: {initial_guess}")
+        # self.get_logger().info(f"Bounds: {bounds}")
 
         try:
             def error_function(vars):
@@ -566,6 +566,7 @@ class UWBNode(Node):
         dy = target_pos[1] - robot_pos[1]
         uwb_heading = math.atan2(dy, dx)
         offset = imu_yaw - uwb_heading
+
         return offset
 
     def get_node_position(self, node_name):
@@ -686,6 +687,12 @@ class UWBNode(Node):
         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")
+        if uwb_heading >= math.pi:
+            uwb_heading = uwb_heading *-1 - math.pi
+        else:
+
+
+
 
         
         self.calibration_active = False