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