Skip to content
Snippets Groups Projects
Commit 71d88d45 authored by Paul-Winpenny's avatar Paul-Winpenny
Browse files

Removed even more comments!

parent d4c4332f
No related branches found
No related tags found
No related merge requests found
...@@ -304,15 +304,15 @@ class UWBNode(Node): ...@@ -304,15 +304,15 @@ class UWBNode(Node):
self.get_logger().warning("Insufficient beacons for trilateration.") self.get_logger().warning("Insufficient beacons for trilateration.")
return None return None
self.get_logger().info(f"Available beacons: {available_beacons}") # self.get_logger().info(f"Available beacons: {available_beacons}")
self.get_logger().info(f"Available distances: {available_distances}") # self.get_logger().info(f"Available distances: {available_distances}")
heights = [] heights = []
for (bx, by, bz), d_measured in zip(available_beacons, available_distances): for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2) horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2)
estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2)) estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2))
heights.append(bz - estimated_z) 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))) initial_z = max(0, min(self.anchorHeight, np.mean(heights)))
...@@ -328,8 +328,8 @@ class UWBNode(Node): ...@@ -328,8 +328,8 @@ class UWBNode(Node):
z_max = self.anchorHeight z_max = self.anchorHeight
bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max]) 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"Initial guess: {initial_guess}")
self.get_logger().info(f"Bounds: {bounds}") # self.get_logger().info(f"Bounds: {bounds}")
try: try:
def error_function(vars): def error_function(vars):
...@@ -566,6 +566,7 @@ class UWBNode(Node): ...@@ -566,6 +566,7 @@ class UWBNode(Node):
dy = target_pos[1] - robot_pos[1] dy = target_pos[1] - robot_pos[1]
uwb_heading = math.atan2(dy, dx) uwb_heading = math.atan2(dy, dx)
offset = imu_yaw - uwb_heading offset = imu_yaw - uwb_heading
return offset return offset
def get_node_position(self, node_name): def get_node_position(self, node_name):
...@@ -686,6 +687,12 @@ class UWBNode(Node): ...@@ -686,6 +687,12 @@ class UWBNode(Node):
self.get_logger().info(f" UWB heading: {uwb_heading:.3f} rad") 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" IMU yaw: {calib_end_yaw:.3f} rad")
self.get_logger().info(f" --> OFFSET: {offset:.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 self.calibration_active = False
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment