From 9b13275c15cbb1aba0cbf9a6b78e361f7a5ed850 Mon Sep 17 00:00:00 2001
From: jlKronos01 <joelleungyancheuk@gmail.com>
Date: Wed, 18 Dec 2024 21:44:40 +0000
Subject: [PATCH] Update uwb_navigation_node.py
---
ros2/src/robobin/robobin/uwb_navigation_node.py | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/ros2/src/robobin/robobin/uwb_navigation_node.py b/ros2/src/robobin/robobin/uwb_navigation_node.py
index 62a347cb..925b464d 100644
--- a/ros2/src/robobin/robobin/uwb_navigation_node.py
+++ b/ros2/src/robobin/robobin/uwb_navigation_node.py
@@ -308,10 +308,10 @@ class UWBNode(Node):
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
--
GitLab