Skip to content
Snippets Groups Projects
Commit 5b338480 authored by jlKronos01's avatar jlKronos01
Browse files

Moved graph maker and rtl cli into ros2 folder

parent bbfaceac
No related branches found
No related tags found
No related merge requests found
...@@ -73,7 +73,7 @@ class GraphMaker: ...@@ -73,7 +73,7 @@ class GraphMaker:
print(f"Node {new_node_index+1} added at ({tag1_x:.2f}, {tag1_y:.2f}).") print(f"Node {new_node_index+1} added at ({tag1_x:.2f}, {tag1_y:.2f}).")
def save_graph(self): 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 os.makedirs(directory, exist_ok=True) # Create the directory if it doesn't exist
file_path = os.path.join(directory, "graph.json") file_path = os.path.join(directory, "graph.json")
with open(file_path, "w") as f: with open(file_path, "w") as f:
......
...@@ -70,6 +70,7 @@ class AnchorTagCLI: ...@@ -70,6 +70,7 @@ class AnchorTagCLI:
self.tag2_distances = {"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0} self.tag2_distances = {"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0}
self.anchors = {} self.anchors = {}
self.anchorHeight = 250
self.anchors_coords_known = False self.anchors_coords_known = False
if self.testing: if self.testing:
...@@ -86,25 +87,30 @@ class AnchorTagCLI: ...@@ -86,25 +87,30 @@ class AnchorTagCLI:
def determine_anchor_coords(self): def determine_anchor_coords(self):
try: try:
measured_distances = np.array(self.measured_distances) measured_distances = np.array(self.measured_distances)
noise_level = 0.0 y_A = measured_distances[2] # Distance from A to D
measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances))
# Variables: x_B, y_B, x_C, y_C, y_A # Guess for B based on distance A-B and B-D
initial_guess = [120, 100, 150, 200, 50] # [x_B, y_B, x_C, y_C, y_A] x_B = measured_distances[0] / 2
maxBounds = 30000 y_B = measured_distances[4] / 2 + 200 # Start y_B above y_C
bounds = ([0, 0, 0, 0, 0], [maxBounds] * 5)
# 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): def error_function(variables, measured):
x_B, y_B, x_C, y_C, y_A = variables x_B, y_B, x_C, y_C, y_A = variables
# measured: [A, E, D, B, F, C]
a_measured = measured[0] # Map measured distances to a, e, d, b, f, c
e_measured = measured[1] a_measured, e_measured, d_measured, b_measured, f_measured, c_measured = measured
d_measured = measured[2]
b_measured = measured[3] # Compute each distance
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)
a_calc = np.sqrt((x_B - 0)**2 + (y_B - y_A)**2) # A-B 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 c_calc = np.sqrt(x_C**2 + y_C**2) # C-D
...@@ -112,30 +118,27 @@ class AnchorTagCLI: ...@@ -112,30 +118,27 @@ class AnchorTagCLI:
e_calc = np.sqrt(x_C**2 + (y_C - y_A)**2) # A-C 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 f_calc = np.sqrt(x_B**2 + y_B**2) # B-D
return [ # Residuals
a_calc - a_measured, r_a = a_calc - a_measured
b_calc - b_measured, r_b = b_calc - b_measured
c_calc - c_measured, r_c = c_calc - c_measured
d_calc - d_measured, r_d = d_calc - d_measured
e_calc - e_measured, r_e = e_calc - e_measured
f_calc - f_measured r_f = 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
# 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 = { self.anchors = {
"A1": (0, optimized_coords_noisy[4]), "A1": (0, y_A, self.anchorHeight),
"A2": (optimized_coords_noisy[0], optimized_coords_noisy[1]), "A2": (x_B, y_B, self.anchorHeight),
"A3": (optimized_coords_noisy[2], optimized_coords_noisy[3]), "A3": (x_C, y_C, self.anchorHeight),
"A4": (0, 0), "A4": (0, 0, self.anchorHeight)
} }
return {k: (round(v[0], 2), round(v[1], 2)) for k, v in self.anchors.items()} return {k: (round(v[0], 2), round(v[1], 2)) for k, v in self.anchors.items()}
except Exception as e: except Exception as e:
...@@ -155,27 +158,39 @@ class AnchorTagCLI: ...@@ -155,27 +158,39 @@ class AnchorTagCLI:
if len(available_beacons) < 3: if len(available_beacons) < 3:
return None, None return None, None
def error_function(vars): heights = []
x, y = vars for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
residuals = [] horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2) # Assume tag starts at (0, 0)
for (bx, by), d_measured in zip(available_beacons, available_distances): estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2)) # Ensure non-negative
d_computed = np.sqrt((x - bx)**2 + (y - by)**2) heights.append(bz - estimated_z) # Estimate tag height relative to beacon
residuals.append(d_computed - d_measured)
return residuals 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_xs = [b[0] for b in available_beacons]
beacon_ys = [b[1] 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_guess = [np.mean(beacon_xs), np.mean(beacon_ys), initial_z]
x_min = min(beacon_xs) - 100 # Define bounds for the tag position
x_max = max(beacon_xs) + 100 x_min = min(beacon_xs) - 1000 # Add a small margin around the beacons
y_min = min(beacon_ys) - 100 x_max = max(beacon_xs) + 1000
y_max = max(beacon_ys) + 100 y_min = min(beacon_ys) - 1000
bounds = ([x_min, y_min], [x_max, y_max]) 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, z= vars
residuals = []
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
result = least_squares(error_function, initial_guess, bounds=bounds, loss='soft_l1') result = least_squares(error_function, initial_guess, bounds=bounds, loss='soft_l1')
x_tag, y_tag = result.x # self.get_logger().info(f"Optimization result: {result.x}")
return x_tag, y_tag return tuple(result.x)
def call_bpm(self): def call_bpm(self):
if self.testing: if self.testing:
...@@ -216,24 +231,24 @@ class AnchorTagCLI: ...@@ -216,24 +231,24 @@ class AnchorTagCLI:
self.tag2_distances[anchor] = distance self.tag2_distances[anchor] = distance
# Now calculate both tags # 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] valid_tag2_distances = [dist for dist in self.tag2_distances.values() if dist > 0]
# Check if there are enough valid distances for Tag 2 # Check if there are enough valid distances for Tag 2
if len(valid_tag2_distances) < 3: if len(valid_tag2_distances) < 3:
print(f"Insufficient valid distances for Tag 2: {len(valid_tag2_distances)} provided.") 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: 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:") print("Tag Positions:")
if tag1_x is not None and tag1_y is not None: 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: else:
print("Tag 1: Not enough data") print("Tag 1: Not enough data")
if tag2_x is not None and tag2_y is not None: 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: else:
print("Tag 2: Not enough data") print("Tag 2: Not enough data")
...@@ -241,7 +256,8 @@ class AnchorTagCLI: ...@@ -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: 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 dx = tag2_x - tag1_x
dy = tag2_y - tag1_y 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)) angle_deg = np.degrees(np.arctan2(dy, dx))
if angle_deg < 0: if angle_deg < 0:
angle_deg += 360 angle_deg += 360
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment