diff --git a/ros2/src/robobin/robobin/helpers/graph_maker.py b/ros2/src/robobin/robobin/helpers/graph_maker.py index c197c10b7eea1a63721b6cf40554a57b79c13d75..202fc34fbdb12d8307c22f7da6bdfc946c351040 100644 --- a/ros2/src/robobin/robobin/helpers/graph_maker.py +++ b/ros2/src/robobin/robobin/helpers/graph_maker.py @@ -73,7 +73,7 @@ class GraphMaker: print(f"Node {new_node_index+1} added at ({tag1_x:.2f}, {tag1_y:.2f}).") def save_graph(self): - directory = r"D:\Github\robobin\ros2\src\robobin\robobin\graphs" #Replace with whereever your graphs folder is in the ros2 workspace + directory = "/Users/paulwinpenny/Documents/GitHub/robobin/Wireless_Communication/UWB/Beacons_tag_position/output" os.makedirs(directory, exist_ok=True) # Create the directory if it doesn't exist file_path = os.path.join(directory, "graph.json") with open(file_path, "w") as f: diff --git a/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py b/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py index a314cec3db27597e1303cf8ca2d0a00523600dbe..2c9284f5b76954d74374a8da44127e6f7db673fe 100644 --- a/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py +++ b/ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py @@ -70,6 +70,7 @@ class AnchorTagCLI: self.tag2_distances = {"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0} self.anchors = {} + self.anchorHeight = 250 self.anchors_coords_known = False if self.testing: @@ -86,56 +87,58 @@ class AnchorTagCLI: def determine_anchor_coords(self): try: measured_distances = np.array(self.measured_distances) - noise_level = 0.0 - measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances)) + y_A = measured_distances[2] # Distance from A to D - # Variables: x_B, y_B, x_C, y_C, y_A - initial_guess = [120, 100, 150, 200, 50] # [x_B, y_B, x_C, y_C, y_A] - maxBounds = 30000 - bounds = ([0, 0, 0, 0, 0], [maxBounds] * 5) + # Guess for B based on distance A-B and B-D + x_B = measured_distances[0] / 2 + y_B = measured_distances[4] / 2 + 200 # Start y_B above y_C + + # Guess for C with symmetrical logic + x_C = -measured_distances[5] / 2 # Allow for negative x_C + y_C = -measured_distances[1] / 2 # Allow for negative y_C + + initial_guess = [x_B, y_B, x_C, y_C, y_A] + + min_dist = min(measured_distances) + max_dist = max(measured_distances) + lower_bounds = [-max_dist, -max_dist, -max_dist, -max_dist, min_dist / 2] + upper_bounds = [max_dist * 1.5 for i in range(5)] def error_function(variables, measured): x_B, y_B, x_C, y_C, y_A = variables - # measured: [A, E, D, B, F, C] - a_measured = measured[0] - e_measured = measured[1] - d_measured = measured[2] - b_measured = measured[3] - f_measured = measured[4] - c_measured = measured[5] - - # A=(0,y_A), B=(x_B,y_B), C=(x_C,y_C), D=(0,0) + + # Map measured distances to a, e, d, b, f, c + a_measured, e_measured, d_measured, b_measured, f_measured, c_measured = measured + + # Compute each distance a_calc = np.sqrt((x_B - 0)**2 + (y_B - y_A)**2) # A-B - b_calc = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2) # B-C + b_calc = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2) # B-C c_calc = np.sqrt(x_C**2 + y_C**2) # C-D d_calc = y_A # A-D e_calc = np.sqrt(x_C**2 + (y_C - y_A)**2) # A-C f_calc = np.sqrt(x_B**2 + y_B**2) # B-D - return [ - a_calc - a_measured, - b_calc - b_measured, - c_calc - c_measured, - d_calc - d_measured, - e_calc - e_measured, - f_calc - f_measured - ] - - # Run least squares optimization - result_noisy = least_squares( - error_function, - initial_guess, - args=(measured_distances_noisy,), - bounds=bounds, - loss='soft_l1' - ) - optimized_coords_noisy = result_noisy.x + # Residuals + r_a = a_calc - a_measured + r_b = b_calc - b_measured + r_c = c_calc - c_measured + r_d = d_calc - d_measured + r_e = e_calc - e_measured + r_f = f_calc - f_measured + + # Add a smoother penalty if y_B <= y_C + penalty = 1e3 * max(0, y_C - y_B + 10) # Soft penalty to enforce constraint + + return [r_a, r_b, r_c, r_d, r_e, r_f, penalty] + + result = least_squares(error_function, initial_guess, args=(measured_distances,), bounds=(lower_bounds, upper_bounds), loss='soft_l1') + x_B, y_B, x_C, y_C, y_A = result.x self.anchors = { - "A1": (0, optimized_coords_noisy[4]), - "A2": (optimized_coords_noisy[0], optimized_coords_noisy[1]), - "A3": (optimized_coords_noisy[2], optimized_coords_noisy[3]), - "A4": (0, 0), + "A1": (0, y_A, self.anchorHeight), + "A2": (x_B, y_B, self.anchorHeight), + "A3": (x_C, y_C, self.anchorHeight), + "A4": (0, 0, self.anchorHeight) } return {k: (round(v[0], 2), round(v[1], 2)) for k, v in self.anchors.items()} except Exception as e: @@ -155,27 +158,39 @@ class AnchorTagCLI: if len(available_beacons) < 3: return None, None + heights = [] + for (bx, by, bz), d_measured in zip(available_beacons, available_distances): + horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2) # Assume tag starts at (0, 0) + estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2)) # Ensure non-negative + heights.append(bz - estimated_z) # Estimate tag height relative to beacon + + initial_z = max(0, min(self.anchorHeight, np.mean(heights))) # Constrain height to [0, 200] + + beacon_xs = [b[0] for b in available_beacons] + beacon_ys = [b[1] for b in available_beacons] + initial_guess = [np.mean(beacon_xs), np.mean(beacon_ys), initial_z] + + # Define bounds for the tag position + x_min = min(beacon_xs) - 1000 # Add a small margin around the beacons + x_max = max(beacon_xs) + 1000 + y_min = min(beacon_ys) - 1000 + y_max = max(beacon_ys) + 1000 + z_min = 0.0 # Tag's height cannot be below the ground + z_max = self.anchorHeight # Tag's height cannot exceed the beacon height + bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max]) + def error_function(vars): - x, y = vars + x, y, z= vars residuals = [] - for (bx, by), d_measured in zip(available_beacons, available_distances): - d_computed = np.sqrt((x - bx)**2 + (y - by)**2) + for (bx, by, bz), d_measured in zip(available_beacons, available_distances): + d_computed = np.sqrt((x - bx)**2 + (y - by)**2 + (z - bz)**2) residuals.append(d_computed - d_measured) return residuals - beacon_xs = [b[0] for b in available_beacons] - beacon_ys = [b[1] for b in available_beacons] - initial_guess = [np.mean(beacon_xs), np.mean(beacon_ys)] - - x_min = min(beacon_xs) - 100 - x_max = max(beacon_xs) + 100 - y_min = min(beacon_ys) - 100 - y_max = max(beacon_ys) + 100 - bounds = ([x_min, y_min], [x_max, y_max]) - result = least_squares(error_function, initial_guess, bounds=bounds, loss='soft_l1') - x_tag, y_tag = result.x - return x_tag, y_tag + # self.get_logger().info(f"Optimization result: {result.x}") + return tuple(result.x) + def call_bpm(self): if self.testing: @@ -216,24 +231,24 @@ class AnchorTagCLI: self.tag2_distances[anchor] = distance # Now calculate both tags - tag1_x, tag1_y = self.calculate_tag_coordinates(self.tag_distances) + tag1_x, tag1_y, tag1_z = self.calculate_tag_coordinates(self.tag_distances) valid_tag2_distances = [dist for dist in self.tag2_distances.values() if dist > 0] # Check if there are enough valid distances for Tag 2 if len(valid_tag2_distances) < 3: print(f"Insufficient valid distances for Tag 2: {len(valid_tag2_distances)} provided.") - tag2_x, tag2_y = None, None + tag2_x, tag2_y, tag2_z = None, None, None else: - tag2_x, tag2_y = self.calculate_tag_coordinates(self.tag2_distances) + tag2_x, tag2_y, tag2_z = self.calculate_tag_coordinates(self.tag2_distances) print("Tag Positions:") if tag1_x is not None and tag1_y is not None: - print(f"Tag 1: ({tag1_x:.2f}, {tag1_y:.2f})") + print(f"Tag 1: ({tag1_x:.2f}, {tag1_y:.2f}, {tag1_z:.2f})") else: print("Tag 1: Not enough data") if tag2_x is not None and tag2_y is not None: - print(f"Tag 2: ({tag2_x:.2f}, {tag2_y:.2f})") + print(f"Tag 2: ({tag2_x:.2f}, {tag2_y:.2f}, {tag2_z:.2f})") else: print("Tag 2: Not enough data") @@ -241,7 +256,8 @@ class AnchorTagCLI: if tag1_x is not None and tag1_y is not None and tag2_x is not None and tag2_y is not None: dx = tag2_x - tag1_x dy = tag2_y - tag1_y - displacement = np.sqrt(dx**2 + dy**2) + dz = tag2_z - tag1_z + displacement = np.sqrt(dx**2 + dy**2 + dz**2) angle_deg = np.degrees(np.arctan2(dy, dx)) if angle_deg < 0: angle_deg += 360